X-Git-Url: https://deadsoftware.ru/gitweb?a=blobdiff_plain;f=src%2Fgame%2Fz_aabbtree.pas;h=fcb8f6801d20ea037a5e906b40cc2dcc9cb28c07;hb=5aa51582e429a852142d419b1e3c23d93d31bc0b;hp=73f02f7cac9fb8faf88f7ce753ec5def78051f33;hpb=51420f985c7642cda726ec6e333b4283d3bebd22;p=d2df-sdl.git diff --git a/src/game/z_aabbtree.pas b/src/game/z_aabbtree.pas index 73f02f7..fcb8f68 100644 --- a/src/game/z_aabbtree.pas +++ b/src/game/z_aabbtree.pas @@ -14,16 +14,19 @@ * along with this program. If not, see . *) {$INCLUDE ../shared/a_modes.inc} -{$DEFINE aabbtree_many_asserts} +{.$DEFINE aabbtree_many_asserts} {$DEFINE aabbtree_query_count} +{.$DEFINE aabbtree_use_floats} unit z_aabbtree; interface +uses e_log; + + // ////////////////////////////////////////////////////////////////////////// // type - Float = Single; - PFloat = ^Float; + {$IFDEF aabbtree_use_floats}TreeNumber = Single;{$ELSE}TreeNumber = Integer;{$ENDIF} TTreeFlesh = TObject; @@ -32,56 +35,66 @@ type type Ray2D = record public - origX, origY: Float; - dirX, dirY: Float; + origX, origY: Single; + dirX, dirY: Single; public - procedure normalizeDir (); + constructor Create (ax, ay: Single; aangle: Single); overload; + constructor Create (ax0, ay0, ax1, ay1: Single); overload; + constructor Create (const aray: Ray2D); overload; + + procedure copyFrom (const aray: Ray2D); inline; - procedure setXYAngle (ax, ay: Float; aangle: Float); inline; - procedure setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Float); inline; + procedure normalizeDir (); inline; + + procedure setXYAngle (ax, ay: Single; aangle: Single); inline; + procedure setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Single); inline; end; // ////////////////////////////////////////////////////////////////////////// // type AABB2D = record public - minX, minY, maxX, maxY: Float; + minX, minY, maxX, maxY: TreeNumber; private function getvalid (): Boolean; inline; - function getcenterX (): Float; inline; - function getcenterY (): Float; inline; - function getextentX (): Float; inline; - function getextentY (): Float; inline; + function getcenterX (): TreeNumber; inline; + function getcenterY (): TreeNumber; inline; + function getextentX (): TreeNumber; inline; + function getextentY (): TreeNumber; inline; public - procedure setX0Y0X1Y1 (x0, y0, x1, y1: Float); inline; - procedure setXYWH (ax, ay, aw, ah: Float); inline; + constructor Create (x0, y0, x1, y1: TreeNumber); overload; + constructor Create (const aabb: AABB2D); overload; + constructor Create (const aabb0, aabb1: AABB2D); overload; + + procedure copyFrom (const aabb: AABB2D); inline; + procedure setDims (x0, y0, x1, y1: TreeNumber); inline; - procedure setMergeTwo (var aabb0, aabb1: AABB2D); inline; + procedure setMergeTwo (const aabb0, aabb1: AABB2D); inline; - function volume (): Float; inline; + function volume (): TreeNumber; inline; - procedure merge (var aabb: AABB2D); inline; + procedure merge (const 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 (ax, ay: Float): Boolean; inline; overload; + function contains (const 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 (const aabb: AABB2D): Boolean; inline; overload; // ray direction must be normalized - function intersects (var ray: Ray2D; tmino: PFloat=nil; tmaxo: PFloat=nil): Boolean; overload; - function intersects (ax, ay, bx, by: Float): Boolean; overload; + function intersects (const ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload; + function intersects (ax, ay, bx, by: Single): Boolean; inline; overload; property valid: Boolean read getvalid; - property centerX: Float read getcenterX; - property centerY: Float read getcenterY; - property extentX: Float read getextentX; - property extentY: Float read getextentY; + property centerX: TreeNumber read getcenterX; + property centerY: TreeNumber read getcenterY; + property extentX: TreeNumber read getextentX; + property extentY: TreeNumber read getextentY; end; @@ -116,35 +129,6 @@ type * based on the one from Erin Catto in Box2D as described in the book * "Introduction to Game Physics with Box2D" by Ian Parberry. *) -type - PDTProxyRec = ^TDTProxyRec; - TDTProxyRec = record - private - mX, mY, mWidth, mHeight: Integer; - mQueryMark: DWord; // was this object visited at this query? - mObj: TObject; - mTag: Integer; - nextfree: Integer; - - private - procedure setup (aX, aY, aWidth, aHeight: Integer; aObj: TObject; aTag: Integer); - - function getx1 (): Integer; inline; - function gety1 (): Integer; inline; - - public - property x: Integer read mX; - property y: Integer read mY; - property width: Integer read mWidth; - property height: Integer read mHeight; - property x0: Integer read mX; - property y0: Integer read mY; - property x1: Integer read getx1; - property y1: Integer read gety1; - property obj: TObject read mObj; - property tag: Integer read mTag; - end; - // ////////////////////////////////////////////////////////////////////////// // // Dynamic AABB Tree: can be used to speed up broad phase in various engines type @@ -169,17 +153,18 @@ type height: SmallInt; // fat axis aligned bounding box (AABB) corresponding to the node aabb: AABB2D; + tag: Integer; // just a user-defined tag public // return true if the node is a leaf of the tree - procedure clear (); + procedure clear (); inline; function leaf (): Boolean; inline; - function free (): Boolean; inline; + function isfree (): Boolean; inline; property nextNodeId: Integer read parentId write parentId; //property flesh: Integer read children[0] write children[0]; end; TVisitCheckerCB = function (node: PTreeNode): Boolean is nested; - TVisitVisitorCB = function (abody: TTreeFlesh): Boolean is nested; + //TVisitVisitorCB = function (abody: TTreeFlesh; atag: Integer): Boolean is nested; public // return `true` to stop @@ -189,7 +174,11 @@ type // in the broad-phase collision detection (dynamic AABB tree), the AABBs are // also inflated in direction of the linear motion of the body by mutliplying the // followin constant with the linear velocity and the elapsed time between two frames - const LinearMotionGapMultiplier = Float(1.7); + {$IFDEF aabbtree_use_floats} + const LinearMotionGapMultiplier = 1.7; + {$ELSE} + const LinearMotionGapMultiplier = 17; // *10 + {$ENDIF} private mNodes: array of TTreeNode; // nodes of the tree @@ -200,7 +189,21 @@ type // 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 - mExtraGap: Float; + 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 + + TSegmentQueryResult = record + dist: Single; // <0: nothing was hit + flesh: TTreeFlesh; + + procedure reset (); inline; + function valid (): Boolean; inline; + end; private function allocateNode (): Integer; @@ -211,29 +214,15 @@ type function computeHeight (nodeId: Integer): Integer; function insertObjectInternal (var aabb: AABB2D; staticObject: Boolean): Integer; procedure setup (); - function visit (checker: TVisitCheckerCB; visitor: TVisitVisitorCB): Integer; + function visit (var caabb: AABB2D; mode: Integer; checker: TVisitCheckerCB; visitor: TQueryOverlapCB; tagmask: Integer=-1): Integer; public {$IFDEF aabbtree_query_count} - nodesVisited, nodesDeepVisited: Integer; + mNodesVisited, mNodesDeepVisited: Integer; {$ENDIF} public - // called when a overlapping node has been found during the call to forEachAABBOverlap() - // return `true` to stop - type TQueryOverlapCB = function (abody: TTreeFlesh): Boolean is nested; - type TSegQueryCallback = function (abody: TTreeFlesh; ax, ay, bx, by: Float): Float is nested; // return dist from (ax,ay) to abody - - TSegmentQueryResult = record - dist: Float; // <0: nothing was hit - flesh: TTreeFlesh; - - procedure reset (); inline; - function valid (): Boolean; inline; - end; - - public - constructor Create (extraAABBGap: Float=0.0); + constructor Create (extraAABBGap: TreeNumber=0); destructor Destroy (); override; // clear all the nodes and reset the tree @@ -253,7 +242,7 @@ type // 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; staticObject: Boolean=false): Integer; + function insertObject (flesh: TTreeFlesh; tag: Integer; staticObject: Boolean=false): Integer; // remove an object from the tree // WARNING: ids of removed objects can be reused on later insertions! @@ -274,17 +263,24 @@ type * * return `true` if the tree was modified. *) - function updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean; + function updateObject (nodeId: Integer; dispX, dispY: TreeNumber; forceReinsert: Boolean=false): Boolean; - procedure aabbQuery (ax, ay, aw, ah: Float; cb: TQueryOverlapCB); - function pointQuery (ax, ay: Float; cb: TQueryOverlapCB): TTreeFlesh; - function segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean; + 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 computeTreeHeight (): Integer; // compute the height of the tree - property extraGap: Float read mExtraGap write mExtraGap; + property extraGap: TreeNumber read mExtraGap write mExtraGap; property nodeCount: Integer read mNodeCount; property nodeAlloced: Integer read mAllocCount; + {$IFDEF aabbtree_query_count} + property nodesVisited: Integer read mNodesVisited; + property nodesDeepVisited: Integer read mNodesDeepVisited; + {$ELSE} + const nodesVisited = 0; + const nodesDeepVisited = 0; + {$ENDIF} end; @@ -298,35 +294,34 @@ 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 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; + // ////////////////////////////////////////////////////////////////////////// // -procedure TDTProxyRec.setup (aX, aY, aWidth, aHeight: Integer; aObj: TObject; aTag: Integer); -begin - mX := aX; - mY := aY; - mWidth := aWidth; - mHeight := aHeight; - mQueryMark := 0; - mObj := aObj; - mTag := aTag; - nextfree := -1; -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 (const aray: Ray2D); overload; begin copyFrom(aray); end; -function TDTProxyRec.getx1 (): Integer; begin result := mX+mWidth-1; end; -function TDTProxyRec.gety1 (): Integer; begin result := mY+mHeight-1; end; +procedure Ray2D.copyFrom (const aray: Ray2D); inline; +begin + origX := aray.origX; + origY := aray.origY; + dirX := aray.dirX; + dirY := aray.dirY; +end; -// ////////////////////////////////////////////////////////////////////////// // -procedure Ray2D.normalizeDir (); +procedure Ray2D.normalizeDir (); inline; var - invlen: Float; + invlen: Single; begin - invlen := Float(1.0)/sqrt(dirX*dirX+dirY*dirY); + invlen := 1.0/sqrt(dirX*dirX+dirY*dirY); dirX *= invlen; dirY *= invlen; end; -procedure Ray2D.setXYAngle (ax, ay: Float; aangle: Float); +procedure Ray2D.setXYAngle (ax, ay: Single; aangle: Single); inline; begin origX := ax; origY := ay; @@ -334,7 +329,7 @@ begin dirY := sin(aangle); end; -procedure Ray2D.setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Float); +procedure Ray2D.setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Single); inline; begin origX := ax0; origY := ay0; @@ -345,69 +340,95 @@ end; // ////////////////////////////////////////////////////////////////////////// // -function AABB2D.getvalid (): Boolean; begin result := (minX <= maxX) and (minY <= maxY); end; - -function AABB2D.getcenterX (): Float; begin result := (minX+maxX)/2; end; -function AABB2D.getcenterY (): Float; begin result := (minY+maxY)/2; end; -function AABB2D.getextentX (): Float; begin result := (maxX-minX)+1; end; -function AABB2D.getextentY (): Float; begin result := (maxY-minY)+1; end; +constructor AABB2D.Create (x0, y0, x1, y1: TreeNumber); overload; +begin + setDims(x0, y0, x1, y1); +end; +constructor AABB2D.Create (const aabb: AABB2D); overload; +begin + copyFrom(aabb); +end; -procedure AABB2D.setX0Y0X1Y1 (x0, y0, x1, y1: Float); +constructor AABB2D.Create (const aabb0, aabb1: AABB2D); overload; begin - if (x0 < x1) then begin minX := x0; maxX := x1; end else begin minX := x1; maxX := x0; end; - if (y0 < y1) then begin minY := y0; maxY := y1; end else begin minY := y1; maxY := y0; end; + setMergeTwo(aabb0, aabb1); end; +function AABB2D.getvalid (): Boolean; inline; begin result := (minX < maxX) and (minY < maxY); end; -procedure AABB2D.setXYWH (ax, ay, aw, ah: Float); +{$IFDEF aabbtree_use_floats} +function AABB2D.getcenterX (): TreeNumber; inline; begin result := (minX+maxX)/2.0; end; +function AABB2D.getcenterY (): TreeNumber; inline; begin result := (minY+maxY)/2.0; end; +{$ELSE} +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; + +procedure AABB2D.copyFrom (const aabb: AABB2D); inline; begin - if (aw < 0) then aw := 0; - if (ah < 0) then ah := 0; - minX := ax; - minY := ay; - maxX := ax+aw-1; - maxY := ay+ah-1; + minX := aabb.minX; + minY := aabb.minY; + maxX := aabb.maxX; + maxY := aabb.maxY; + {$IF DEFINED(D2F_DEBUG)} + if not valid then raise Exception.Create('copyFrom: result is fucked'); + {$ENDIF} end; -procedure AABB2D.setMergeTwo (var aabb0, aabb1: AABB2D); -var - x0, y0, x1, y1: Float; +procedure AABB2D.setDims (x0, y0, x1, y1: TreeNumber); inline; begin - if (aabb0.minX < aabb1.minX) then x0 := aabb0.minX else x0 := aabb1.minX; - if (aabb0.minY < aabb1.minY) then y0 := aabb0.minY else y0 := aabb1.minY; + minX := minF(x0, x1); + minY := minF(y0, y1); + maxX := maxF(x0, x1); + maxY := maxF(y0, y1); + {$IF DEFINED(D2F_DEBUG)} + if not valid then raise Exception.Create('setDims: result is fucked'); + {$ENDIF} +end; - if (aabb0.maxX > aabb1.maxX) then x1 := aabb0.maxX else x1 := aabb1.maxX; - if (aabb0.maxY > aabb1.maxY) then y1 := aabb0.maxY else y1 := aabb1.maxY; - minX := x0; - minY := y0; - maxX := x1; - maxY := y1; +procedure AABB2D.setMergeTwo (const 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); + {$IF DEFINED(D2F_DEBUG)} + if not valid then raise Exception.Create('setMergeTwo: result is fucked'); + {$ENDIF} end; -function AABB2D.volume (): Float; -var - diffX, diffY: Float; +function AABB2D.volume (): TreeNumber; inline; begin - diffX := maxX-minX; - diffY := maxY-minY; - result := diffX*diffY; + result := (maxX-minX)*(maxY-minY); end; -procedure AABB2D.merge (var aabb: AABB2D); +procedure AABB2D.merge (const aabb: AABB2D); inline; begin - if (minX > aabb.minX) then minX := aabb.minX; - if (minY > aabb.minY) then minY := aabb.minY; - if (maxX < aabb.maxX) then maxX := aabb.maxX; - if (maxY < aabb.maxY) then maxY := aabb.maxY; + {$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); + {$IF DEFINED(D2F_DEBUG)} + if not valid then raise Exception.Create('setMergeTwo: result is fucked'); + {$ENDIF} end; -function AABB2D.contains (var aabb: AABB2D): Boolean; overload; +function AABB2D.contains (const aabb: AABB2D): Boolean; inline; overload; begin result := (aabb.minX >= minX) and (aabb.minY >= minY) and @@ -415,13 +436,13 @@ begin end; -function AABB2D.contains (ax, ay: Float): Boolean; overload; +function AABB2D.contains (ax, ay: TreeNumber): Boolean; inline; overload; begin result := (ax >= minX) and (ay >= minY) and (ax <= maxX) and (ay <= maxY); end; -function AABB2D.overlaps (var aabb: AABB2D): Boolean; overload; +function AABB2D.overlaps (const aabb: AABB2D): Boolean; inline; overload; begin result := false; // exit with no intersection if found separated along any axis @@ -433,10 +454,10 @@ 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: PFloat=nil; tmaxo: PFloat=nil): Boolean; overload; +function AABB2D.intersects (const ray: Ray2D; tmino: PSingle=nil; tmaxo: PSingle=nil): Boolean; overload; var - dinv, t1, t2, tmp: Float; - tmin, tmax: Float; + dinv, t1, t2, tmp: Single; + tmin, tmax: Single; begin // ok with coplanars tmin := -1.0e100; @@ -444,7 +465,7 @@ begin // do X if (ray.dirX <> 0.0) then begin - dinv := Float(1.0)/ray.dirX; + dinv := 1.0/ray.dirX; t1 := (minX-ray.origX)*dinv; t2 := (maxX-ray.origX)*dinv; if (t1 < t2) then tmin := t1 else tmin := t2; @@ -453,7 +474,7 @@ begin // do Y if (ray.dirY <> 0.0) then begin - dinv := Float(1.0)/ray.dirY; + dinv := 1.0/ray.dirY; t1 := (minY-ray.origY)*dinv; t2 := (maxY-ray.origY)*dinv; // tmin @@ -478,9 +499,9 @@ begin end; end; -function AABB2D.intersects (ax, ay, bx, by: Float): Boolean; overload; +function AABB2D.intersects (ax, ay, bx, by: Single): Boolean; inline; overload; var - tmin: Float; + tmin: Single; ray: Ray2D; begin result := true; @@ -488,7 +509,7 @@ begin 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.setX0Y0X1Y1(ax, ay, bx, by); + ray := Ray2D.Create(ax, ay, bx, by); if not intersects(ray, @tmin) then begin result := false; exit; end; if (tmin < 0) then exit; // inside, just in case bx := bx-ax; @@ -498,22 +519,26 @@ end; // ////////////////////////////////////////////////////////////////////////// // -procedure TDynAABBTree.TSegmentQueryResult.reset (); begin dist := -1; flesh := nil; end; -function TDynAABBTree.TSegmentQueryResult.valid (): Boolean; begin result := (dist >= 0) and (flesh <> nil); 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; // ////////////////////////////////////////////////////////////////////////// // -function TDynAABBTree.TTreeNode.leaf (): Boolean; begin result := (height = 0); end; -function TDynAABBTree.TTreeNode.free (): Boolean; begin result := (height = -1); end; +function TDynAABBTree.TTreeNode.leaf (): Boolean; inline; begin result := (height = 0); end; +function TDynAABBTree.TTreeNode.isfree (): Boolean; inline; begin result := (height = -1); end; -procedure TDynAABBTree.TTreeNode.clear (); +procedure TDynAABBTree.TTreeNode.clear (); inline; begin parentId := 0; children[0] := 0; children[1] := 0; flesh := nil; + tag := 0; height := 0; - aabb.setX0Y0X1Y1(0, 0, 0, 0); + aabb.minX := 0; + aabb.minY := 0; + aabb.maxX := 0; + aabb.maxY := 0; end; @@ -522,31 +547,33 @@ end; function TDynAABBTree.allocateNode (): Integer; var i, newsz, freeNodeId: Integer; + node: PTreeNode; begin // if there is no more allocated node to use if (mFreeNodeId = TTreeNode.NullTreeNode) then begin {$IFDEF aabbtree_many_asserts}assert(mNodeCount = mAllocCount);{$ENDIF} // allocate more nodes in the tree - if (mAllocCount < 8192) then newsz := mAllocCount*2 else newsz := mAllocCount+8192; + if (mAllocCount < 32768) then newsz := mAllocCount*2 else newsz := mAllocCount+16384; SetLength(mNodes, newsz); mAllocCount := newsz; // initialize the allocated nodes - for i := mNodeCount to mAllocCount-2 do + for i := mNodeCount to mAllocCount-1 do begin mNodes[i].nextNodeId := i+1; mNodes[i].height := -1; end; mNodes[mAllocCount-1].nextNodeId := TTreeNode.NullTreeNode; - mNodes[mAllocCount-1].height := -1; mFreeNodeId := mNodeCount; end; // get the next free node freeNodeId := mFreeNodeId; {$IFDEF aabbtree_many_asserts}assert((freeNodeId >= mNodeCount) and (freeNodeId < mAllocCount));{$ENDIF} - mFreeNodeId := mNodes[freeNodeId].nextNodeId; - mNodes[freeNodeId].parentId := TTreeNode.NullTreeNode; - mNodes[freeNodeId].height := 0; + node := @mNodes[freeNodeId]; + mFreeNodeId := node.nextNodeId; + node.clear(); + node.parentId := TTreeNode.NullTreeNode; + node.height := 0; Inc(mNodeCount); result := freeNodeId; end; @@ -560,6 +587,7 @@ begin {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].height >= 0);{$ENDIF} mNodes[nodeId].nextNodeId := mFreeNodeId; mNodes[nodeId].height := -1; + mNodes[nodeId].flesh := nil; mFreeNodeId := nodeId; Dec(mNodeCount); end; @@ -573,8 +601,8 @@ var currentNodeId: Integer; leftChild, rightChild, siblingNode: Integer; oldParentNode, newParentNode: Integer; - volumeAABB, mergedVolume: Float; - costS, costI, costLeft, costRight: Float; + volumeAABB, mergedVolume: TreeNumber; + costS, costI, costLeft, costRight: TreeNumber; begin // if the tree is empty if (mRootNodeId = TTreeNode.NullTreeNode) then @@ -587,7 +615,7 @@ begin {$IFDEF aabbtree_many_asserts}assert(mRootNodeId <> TTreeNode.NullTreeNode);{$ENDIF} // find the best sibling node for the new node - newNodeAABB := mNodes[nodeId].aabb; + newNodeAABB := AABB2D.Create(mNodes[nodeId].aabb); currentNodeId := mRootNodeId; while not mNodes[currentNodeId].leaf do begin @@ -596,36 +624,24 @@ begin // compute the merged AABB volumeAABB := mNodes[currentNodeId].aabb.volume; - mergedAABBs.setMergeTwo(mNodes[currentNodeId].aabb, newNodeAABB); + mergedAABBs := AABB2D.Create(mNodes[currentNodeId].aabb, newNodeAABB); mergedVolume := mergedAABBs.volume; // compute the cost of making the current node the sibling of the new node - costS := Float(2.0)*mergedVolume; + costS := 2*mergedVolume; // compute the minimum cost of pushing the new node further down the tree (inheritance cost) - costI := Float(2.0)*(mergedVolume-volumeAABB); + costI := 2*(mergedVolume-volumeAABB); // compute the cost of descending into the left child - currentAndLeftAABB.setMergeTwo(newNodeAABB, mNodes[leftChild].aabb); - if (mNodes[leftChild].leaf) then - begin - costLeft := currentAndLeftAABB.volume+costI; - end - else - begin - costLeft := costI+currentAndLeftAABB.volume-mNodes[leftChild].aabb.volume; - end; + currentAndLeftAABB := AABB2D.Create(newNodeAABB, mNodes[leftChild].aabb); + costLeft := currentAndLeftAABB.volume+costI; + if not mNodes[leftChild].leaf then costLeft -= mNodes[leftChild].aabb.volume; // compute the cost of descending into the right child - currentAndRightAABB.setMergeTwo(newNodeAABB, mNodes[rightChild].aabb); - if (mNodes[rightChild].leaf) then - begin - costRight := currentAndRightAABB.volume+costI; - end - else - begin - costRight := costI+currentAndRightAABB.volume-mNodes[rightChild].aabb.volume; - end; + currentAndRightAABB := AABB2D.Create(newNodeAABB, mNodes[rightChild].aabb); + costRight := currentAndRightAABB.volume+costI; + if not mNodes[rightChild].leaf then costRight -= mNodes[rightChild].aabb.volume; // 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 if (costS < costLeft) and (costS < costRight) then break; @@ -986,13 +1002,13 @@ begin nodeId := allocateNode(); // create the fat aabb to use in the tree - mNodes[nodeId].aabb := aabb; + mNodes[nodeId].aabb := AABB2D.Create(aabb); if (not staticObject) then begin - mNodes[nodeId].aabb.minX := mNodes[nodeId].aabb.minX-mExtraGap; - mNodes[nodeId].aabb.minY := mNodes[nodeId].aabb.minY-mExtraGap; - mNodes[nodeId].aabb.maxX := mNodes[nodeId].aabb.maxX+mExtraGap; - mNodes[nodeId].aabb.maxY := mNodes[nodeId].aabb.maxY+mExtraGap; + mNodes[nodeId].aabb.minX -= mExtraGap; + mNodes[nodeId].aabb.minY -= mExtraGap; + mNodes[nodeId].aabb.maxX += mExtraGap; + mNodes[nodeId].aabb.maxY += mExtraGap; end; // set the height of the node in the tree @@ -1023,13 +1039,12 @@ begin for i := 0 to mAllocCount-1 do mNodes[i].clear(); // initialize the allocated nodes - for i := 0 to mAllocCount-2 do + for i := 0 to mAllocCount-1 do begin mNodes[i].nextNodeId := i+1; mNodes[i].height := -1; end; mNodes[mAllocCount-1].nextNodeId := TTreeNode.NullTreeNode; - mNodes[mAllocCount-1].height := -1; mFreeNodeId := 0; end; @@ -1049,12 +1064,30 @@ function TDynAABBTree.forEachLeaf (dg: TForEachLeafCB): Boolean; // get the children nodes pNode := @mNodes[nodeId]; assert(pNode.height >= 0); + if (not pNode.aabb.valid) then + begin + {$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); + {$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); - result := dg(pNode.flesh, pNode.aabb); + if assigned(dg) then result := dg(pNode.flesh, pNode.aabb); end else begin @@ -1070,7 +1103,7 @@ function TDynAABBTree.forEachLeaf (dg: TForEachLeafCB): Boolean; height := 1+maxI(mNodes[leftChild].height, mNodes[rightChild].height); assert(mNodes[nodeId].height = height); // check the AABB of the node - aabb.setMergeTwo(mNodes[leftChild].aabb, mNodes[rightChild].aabb); + 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); @@ -1082,8 +1115,6 @@ function TDynAABBTree.forEachLeaf (dg: TForEachLeafCB): Boolean; end; begin - result := false; - if not assigned(dg) then exit; // recursively check each node result := forEachNode(mRootNodeId); end; @@ -1092,13 +1123,16 @@ 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 -function TDynAABBTree.visit (checker: TVisitCheckerCB; visitor: TVisitVisitorCB): Integer; +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..255] of Integer; // stack with the nodes to visit + stack: array [0..2048] of Integer; // stack with the nodes to visit bigstack: array of Integer = nil; sp: Integer = 0; - procedure spush (id: Integer); + procedure spush (id: Integer); inline; var xsp: Integer; begin @@ -1127,9 +1161,9 @@ var end; end; - function spop (): Integer; + function spop (): Integer; inline; begin - assert(sp > 0); + {$IFDEF aabbtree_many_asserts}assert(sp > 0);{$ENDIF} if (sp <= length(stack)) then begin // use "small stack" @@ -1147,13 +1181,14 @@ var var nodeId: Integer; node: PTreeNode; + doNode: Boolean = false; begin if not assigned(checker) then begin result := -1; exit; end; //if not assigned(visitor) then begin result := -1; exit; end; try {$IFDEF aabbtree_query_count} - nodesVisited := 0; - nodesDeepVisited := 0; + mNodesVisited := 0; + mNodesDeepVisited := 0; {$ENDIF} // start from root node @@ -1166,20 +1201,25 @@ begin nodeId := spop(); // skip it if it is a nil node if (nodeId = TTreeNode.NullTreeNode) then continue; - {$IFDEF aabbtree_query_count}Inc(nodesVisited);{$ENDIF} + {$IFDEF aabbtree_query_count}Inc(mNodesVisited);{$ENDIF} // get the corresponding node node := @mNodes[nodeId]; // should we investigate this node? - if (checker(node)) then + case mode of + ModeNoChecks: doNode := checker(node); + ModeAABB: doNode := caabb.overlaps(node.aabb); + ModePoint: doNode := node.aabb.contains(caabb.minX, caabb.minY); + end; + if doNode then begin // if the node is a leaf if (node.leaf) then begin // call visitor on it - {$IFDEF aabbtree_query_count}Inc(nodesDeepVisited);{$ENDIF} - if assigned(visitor) then + {$IFDEF aabbtree_query_count}Inc(mNodesDeepVisited);{$ENDIF} + if ((node.tag and tagmask) <> 0) and assigned(visitor) then begin - if (visitor(node.flesh)) then begin result := nodeId; exit; end; + if (visitor(node.flesh, node.tag)) then begin result := nodeId; exit; end; end; end else @@ -1200,7 +1240,7 @@ 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: Float=0.0); +constructor TDynAABBTree.Create (extraAABBGap: TreeNumber=0); begin mExtraGap := extraAABBGap; setup(); @@ -1250,7 +1290,7 @@ end; // get fat object AABB by nodeid; returns random shit for invalid ids procedure TDynAABBTree.getNodeFatAABB (var aabb: AABB2D; nodeid: Integer); begin - if (nodeid >= 0) and (nodeid < mNodeCount) and (not mNodes[nodeid].free) then aabb := mNodes[nodeid].aabb else aabb.setX0Y0X1Y1(0, 0, -1, -1); + if (nodeid >= 0) and (nodeid < mNodeCount) and (not mNodes[nodeid].isfree) then aabb.copyFrom(mNodes[nodeid].aabb) else aabb.setDims(0, 0, 0, 0); end; @@ -1258,15 +1298,38 @@ 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; staticObject: Boolean=false): Integer; +function TDynAABBTree.insertObject (flesh: TTreeFlesh; tag: Integer; staticObject: Boolean=false): Integer; var aabb: AABB2D; nodeId: Integer; begin - if not getFleshAABB(aabb, flesh) then begin result := -1; exit; end; + if not getFleshAABB(aabb, flesh) then + begin + {$IFDEF aabbtree_use_floats} + 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); + {$ELSE} + 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); + {$ENDIF} + //raise Exception.Create('trying to insert invalid flesh in dyntree'); + result := -1; + exit; + end; + if not aabb.valid then + begin + {$IFDEF aabbtree_use_floats} + 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); + {$ELSE} + 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); + {$ENDIF} + raise Exception.Create('trying to insert invalid aabb in dyntree'); + result := -1; + exit; + end; + //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); nodeId := insertObjectInternal(aabb, staticObject); {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].leaf);{$ENDIF} mNodes[nodeId].flesh := flesh; + mNodes[nodeId].tag := tag; result := nodeId; end; @@ -1282,13 +1345,14 @@ begin end; -function TDynAABBTree.updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean; +function TDynAABBTree.updateObject (nodeId: Integer; dispX, dispY: TreeNumber; forceReinsert: Boolean=false): Boolean; var newAABB: AABB2D; begin - if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree'); + if (nodeId < 0) or (nodeId >= mNodeCount) or (not mNodes[nodeId].leaf) then raise Exception.Create('invalid node id in TDynAABBTree.updateObject'); - if not getFleshAABB(newAABB, mNodes[nodeId].flesh) then raise Exception.Create('invalid node id in TDynAABBTree'); + if not getFleshAABB(newAABB, mNodes[nodeId].flesh) then raise Exception.Create('invalid node id in TDynAABBTree.updateObject'); + if not newAABB.valid then raise Exception.Create('invalid flesh aabb in TDynAABBTree.updateObject'); // if the new AABB is still inside the fat AABB of the node if (not forceReinsert) and (mNodes[nodeId].aabb.contains(newAABB)) then begin result := false; exit; end; @@ -1298,7 +1362,7 @@ begin // compute the fat AABB by inflating the AABB with a constant gap mNodes[nodeId].aabb := newAABB; - if not forceReinsert and ((dispX <> 0) or (dispY <> 0)) then + if (not forceReinsert) and ((dispX <> 0) or (dispY <> 0)) then begin mNodes[nodeId].aabb.minX := mNodes[nodeId].aabb.minX-mExtraGap; mNodes[nodeId].aabb.minY := mNodes[nodeId].aabb.minY-mExtraGap; @@ -1307,21 +1371,21 @@ begin end; // inflate the fat AABB in direction of the linear motion of the AABB - if (dispX < 0.0) then + if (dispX < 0) then begin - mNodes[nodeId].aabb.minX := mNodes[nodeId].aabb.minX+LinearMotionGapMultiplier*dispX; + mNodes[nodeId].aabb.minX := mNodes[nodeId].aabb.minX+LinearMotionGapMultiplier*dispX {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF}; end else begin - mNodes[nodeId].aabb.maxX := mNodes[nodeId].aabb.maxX+LinearMotionGapMultiplier*dispX; + mNodes[nodeId].aabb.maxX := mNodes[nodeId].aabb.maxX+LinearMotionGapMultiplier*dispX {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF}; end; - if (dispY < 0.0) then + if (dispY < 0) then begin - mNodes[nodeId].aabb.minY := mNodes[nodeId].aabb.minY+LinearMotionGapMultiplier*dispY; + mNodes[nodeId].aabb.minY := mNodes[nodeId].aabb.minY+LinearMotionGapMultiplier*dispY {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF}; end else begin - mNodes[nodeId].aabb.maxY := mNodes[nodeId].aabb.maxY+LinearMotionGapMultiplier*dispY; + mNodes[nodeId].aabb.maxY := mNodes[nodeId].aabb.maxY+LinearMotionGapMultiplier*dispY {$IFDEF aabbtree_use_floats}{$ELSE}div 10{$ENDIF}; end; {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].aabb.contains(newAABB));{$ENDIF} @@ -1334,52 +1398,60 @@ end; // report all shapes overlapping with the AABB given in parameter -procedure TDynAABBTree.aabbQuery (ax, ay, aw, ah: Float; cb: TQueryOverlapCB); +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; +var + nid: Integer; begin + result := nil; if not assigned(cb) then exit; - caabb.setXYWH(ax, ay, aw, ah); - visit(checker, cb); + if (aw < 1) or (ah < 1) then exit; + caabb := AABB2D.Create(ax, ay, ax+aw, ay+ah); + nid := visit(caabb, ModeAABB, checker, cb, tagmask); + if (nid >= 0) then result := mNodes[nid].flesh else result := nil; end; // report body that contains the given point, or nil -function TDynAABBTree.pointQuery (ax, ay: Float; cb: TQueryOverlapCB): TTreeFlesh; -var - nid: Integer; +function TDynAABBTree.pointQuery (ax, ay: TreeNumber; cb: TQueryOverlapCB): TTreeFlesh; function checker (node: PTreeNode): Boolean; begin result := node.aabb.contains(ax, ay); end; +var + nid: Integer; + caabb: AABB2D; begin - nid := visit(checker, cb); + caabb := AABB2D.Create(ax, ay, ax+1, ay+1); + nid := visit(caabb, ModePoint, checker, cb); {$IFDEF aabbtree_many_asserts}assert((nid < 0) or ((nid >= 0) and (nid < mNodeCount) and (mNodes[nid].leaf)));{$ENDIF} if (nid >= 0) then result := mNodes[nid].flesh else result := nil; end; // segment querying method -function TDynAABBTree.segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean; +function TDynAABBTree.segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: TreeNumber; cb: TSegQueryCallback): Boolean; var - maxFraction: Float = 1.0e100; // infinity - curax, curay: Float; - curbx, curby: Float; - dirx, diry: Float; - invlen: Float; + maxFraction: Single = 1.0e100; // infinity + curax, curay: Single; + curbx, curby: Single; + dirx, diry: Single; + invlen: Single; + caabb: AABB2D; function checker (node: PTreeNode): Boolean; begin result := node.aabb.intersects(curax, curay, curbx, curby); end; - function visitor (flesh: TTreeFlesh): Boolean; + function visitor (flesh: TTreeFlesh; tag: Integer): Boolean; var - hitFraction: Float; + hitFraction: Single; begin hitFraction := cb(flesh, curax, curay, curbx, curby); // if the user returned a hitFraction of zero, it means that the raycasting should stop here @@ -1421,11 +1493,12 @@ begin dirx := (curbx-curax); diry := (curby-curay); // normalize - invlen := Float(1.0)/sqrt(dirx*dirx+diry*diry); + invlen := 1.0/sqrt(dirx*dirx+diry*diry); dirx *= invlen; diry *= invlen; - visit(checker, visitor); + caabb := AABB2D.Create(0, 0, 1, 1); + visit(caabb, ModeNoChecks, checker, visitor); result := qr.valid; end;