X-Git-Url: https://deadsoftware.ru/gitweb?a=blobdiff_plain;f=src%2Fgame%2Fz_aabbtree.pas;h=a168a6028e2e319be6d7c1c766dcd9283a5142a7;hb=ae3691d4a1e067e6059b16e727c01daa267800b9;hp=852ae85907aef43276c003fe6b74bfbb9aa20ab7;hpb=28bf5dfda4876c1aea86811c7dec26992bfe6435;p=d2df-sdl.git diff --git a/src/game/z_aabbtree.pas b/src/game/z_aabbtree.pas index 852ae85..a168a60 100644 --- a/src/game/z_aabbtree.pas +++ b/src/game/z_aabbtree.pas @@ -14,8 +14,9 @@ * along with this program. If not, see . *) {$INCLUDE ../shared/a_modes.inc} -{$DEFINE aabbtree_many_asserts} -{$DEFINE aabbtree_query_count} +{.$DEFINE aabbtree_many_asserts} +{.$DEFINE aabbtree_query_count} +{.$DEFINE aabbtree_use_floats} unit z_aabbtree; interface @@ -25,8 +26,8 @@ uses e_log; // ////////////////////////////////////////////////////////////////////////// // type - Float = Single; - PFloat = ^Float; + {$IFDEF aabbtree_use_floats}Float = Single;{$ELSE}Float = Integer;{$ENDIF} + //PFloat = ^Float; TTreeFlesh = TObject; @@ -35,20 +36,20 @@ type type Ray2D = record public - origX, origY: Float; - dirX, dirY: Float; + origX, origY: Single; + dirX, dirY: Single; public - constructor Create (ax, ay: Float; aangle: Float); overload; - constructor Create (ax0, ay0, ax1, ay1: Float); overload; + 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 normalizeDir (); inline; - procedure setXYAngle (ax, ay: Float; aangle: Float); inline; - procedure setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Float); inline; + procedure setXYAngle (ax, ay: Single; aangle: Single); inline; + procedure setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Single); inline; end; // ////////////////////////////////////////////////////////////////////////// // @@ -87,8 +88,8 @@ type function overlaps (const aabb: AABB2D): Boolean; inline; overload; // ray direction must be normalized - function intersects (const ray: Ray2D; tmino: PFloat=nil; tmaxo: PFloat=nil): Boolean; overload; - function intersects (ax, ay, bx, by: Float): Boolean; inline; 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; @@ -153,6 +154,7 @@ 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 (); inline; @@ -163,7 +165,7 @@ type 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 @@ -173,7 +175,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 + {$IFDEF aabbtree_use_floats} const LinearMotionGapMultiplier = 1.7; + {$ELSE} + const LinearMotionGapMultiplier = 17; // *10 + {$ENDIF} private mNodes: array of TTreeNode; // nodes of the tree @@ -186,6 +192,20 @@ type // without triggering a large modification of the tree which can be costly mExtraGap: Float; + 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; procedure releaseNode (nodeId: Integer); @@ -195,29 +215,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 (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: Float=0); destructor Destroy (); override; // clear all the nodes and reset the tree @@ -237,7 +243,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! @@ -260,7 +266,7 @@ type *) function updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean; - function aabbQuery (ax, ay, aw, ah: Float; cb: TQueryOverlapCB): Boolean; + function aabbQuery (ax, ay, aw, ah: Float; cb: TQueryOverlapCB; tagmask: Integer=-1): TTreeFlesh; function pointQuery (ax, ay: Float; cb: TQueryOverlapCB): TTreeFlesh; function segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean; @@ -269,6 +275,13 @@ type property extraGap: Float 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; @@ -287,8 +300,8 @@ function maxF (a, b: Float): Float; inline; begin if (a > b) then result := a el // ////////////////////////////////////////////////////////////////////////// // -constructor Ray2D.Create (ax, ay: Float; aangle: Float); begin setXYAngle(ax, ay, aangle); end; -constructor Ray2D.Create (ax0, ay0, ax1, ay1: Float); begin setX0Y0X1Y1(ax0, ay0, ax1, ay1); 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; @@ -302,14 +315,14 @@ end; procedure Ray2D.normalizeDir (); inline; var - invlen: Float; + invlen: Single; begin invlen := 1.0/sqrt(dirX*dirX+dirY*dirY); dirX *= invlen; dirY *= invlen; end; -procedure Ray2D.setXYAngle (ax, ay: Float; aangle: Float); inline; +procedure Ray2D.setXYAngle (ax, ay: Single; aangle: Single); inline; begin origX := ax; origY := ay; @@ -317,7 +330,7 @@ begin dirY := sin(aangle); end; -procedure Ray2D.setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Float); inline; +procedure Ray2D.setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Single); inline; begin origX := ax0; origY := ay0; @@ -345,11 +358,15 @@ end; function AABB2D.getvalid (): Boolean; inline; begin result := (minX < maxX) and (minY < maxY); end; +{$IFDEF aabbtree_use_floats} function AABB2D.getcenterX (): Float; inline; begin result := (minX+maxX)/2.0; end; function AABB2D.getcenterY (): Float; inline; begin result := (minY+maxY)/2.0; end; -function AABB2D.getextentX (): Float; inline; begin result := (maxX-minX)+1.0; end; -function AABB2D.getextentY (): Float; inline; begin result := (maxY-minY)+1.0; end; - +{$ELSE} +function AABB2D.getcenterX (): Float; inline; begin result := (minX+maxX) div 2; end; +function AABB2D.getcenterY (): Float; inline; begin result := (minY+maxY) div 2; end; +{$ENDIF} +function AABB2D.getextentX (): Float; inline; begin result := (maxX-minX); end; +function AABB2D.getextentY (): Float; inline; begin result := (maxY-minY); end; procedure AABB2D.copyFrom (const aabb: AABB2D); inline; begin @@ -438,10 +455,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 (const 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; @@ -483,9 +500,9 @@ begin end; end; -function AABB2D.intersects (ax, ay, bx, by: Float): Boolean; inline; overload; +function AABB2D.intersects (ax, ay, bx, by: Single): Boolean; inline; overload; var - tmin: Float; + tmin: Single; ray: Ray2D; begin result := true; @@ -517,6 +534,7 @@ begin children[0] := 0; children[1] := 0; flesh := nil; + tag := 0; height := 0; aabb.minX := 0; aabb.minY := 0; @@ -611,10 +629,10 @@ begin mergedVolume := mergedAABBs.volume; // compute the cost of making the current node the sibling of the new node - costS := 2.0*mergedVolume; + costS := 2*mergedVolume; // compute the minimum cost of pushing the new node further down the tree (inheritance cost) - costI := 2.0*(mergedVolume-volumeAABB); + costI := 2*(mergedVolume-volumeAABB); // compute the cost of descending into the left child currentAndLeftAABB := AABB2D.Create(newNodeAABB, mNodes[leftChild].aabb); @@ -799,7 +817,7 @@ begin balanceFactor := nodeC.height-nodeB.height; // if the right node C is 2 higher than left node B - if (balanceFactor > 1.0) then + if (balanceFactor > 1) then begin {$IFDEF aabbtree_many_asserts}assert(not nodeC.leaf);{$ENDIF} @@ -1049,11 +1067,19 @@ function TDynAABBTree.forEachLeaf (dg: TForEachLeafCB): Boolean; 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); @@ -1098,13 +1124,13 @@ 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; +function TDynAABBTree.visit (checker: TVisitCheckerCB; visitor: TQueryOverlapCB; tagmask: Integer=-1): Integer; var stack: array [0..255] 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 @@ -1133,9 +1159,10 @@ 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" @@ -1149,6 +1176,7 @@ var result := bigstack[sp-length(stack)]; end; end; + *) var nodeId: Integer; @@ -1158,8 +1186,8 @@ begin //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 @@ -1169,10 +1197,24 @@ begin while (sp > 0) do begin // get the next node id to visit - nodeId := spop(); + //nodeId := spop(); + {$IFDEF aabbtree_many_asserts}assert(sp > 0);{$ENDIF} + if (sp <= length(stack)) then + begin + // use "small stack" + Dec(sp); + nodeId := stack[sp]; + end + else + begin + // use "big stack" + Dec(sp); + nodeId := bigstack[sp-length(stack)]; + end; + // 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? @@ -1182,10 +1224,10 @@ begin 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 @@ -1206,7 +1248,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: Float=0); begin mExtraGap := extraAABBGap; setup(); @@ -1264,21 +1306,29 @@ 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 + {$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; @@ -1287,6 +1337,7 @@ begin nodeId := insertObjectInternal(aabb, staticObject); {$IFDEF aabbtree_many_asserts}assert(mNodes[nodeId].leaf);{$ENDIF} mNodes[nodeId].flesh := flesh; + mNodes[nodeId].tag := tag; result := nodeId; end; @@ -1328,21 +1379,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} @@ -1355,29 +1406,33 @@ end; // report all shapes overlapping with the AABB given in parameter -function TDynAABBTree.aabbQuery (ax, ay, aw, ah: Float; cb: TQueryOverlapCB): Boolean; +function TDynAABBTree.aabbQuery (ax, ay, aw, ah: Float; 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; if (aw < 1) or (ah < 1) then exit; caabb := AABB2D.Create(ax, ay, ax+aw, ay+ah); - result := (visit(checker, cb) <> -1); + nid := visit(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 checker (node: PTreeNode): Boolean; begin result := node.aabb.contains(ax, ay); end; +var + nid: Integer; begin nid := visit(checker, cb); {$IFDEF aabbtree_many_asserts}assert((nid < 0) or ((nid >= 0) and (nid < mNodeCount) and (mNodes[nid].leaf)));{$ENDIF} @@ -1388,20 +1443,20 @@ end; // segment querying method function TDynAABBTree.segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; 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; 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