index 91a98190c352c73f6d06dd310c9f98007ef3209d..852ae85907aef43276c003fe6b74bfbb9aa20ab7 100644 (file)
--- a/src/game/z_aabbtree.pas
+++ b/src/game/z_aabbtree.pas
+(* Copyright (C) DooM 2D:Forever Developers
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *)
{$INCLUDE ../shared/a_modes.inc}
{$DEFINE aabbtree_many_asserts}
{$DEFINE aabbtree_query_count}
interface
+uses e_log;
+
+
// ////////////////////////////////////////////////////////////////////////// //
type
Float = Single;
PFloat = ^Float;
+ TTreeFlesh = TObject;
+
// ////////////////////////////////////////////////////////////////////////// //
type
dirX, dirY: Float;
public
- procedure normalizeDir ();
+ constructor Create (ax, ay: Float; aangle: Float); overload;
+ constructor Create (ax0, ay0, ax1, ay1: Float); 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;
function getextentY (): Float; inline;
public
- procedure setX0Y0X1Y1 (x0, y0, x1, y1: Float); inline;
- procedure setXYWH (ax, ay, aw, ah: Float); inline;
+ constructor Create (x0, y0, x1, y1: Float); 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: Float); inline;
- procedure setMergeTwo (var aabb0, aabb1: AABB2D); inline;
+ procedure setMergeTwo (const aabb0, aabb1: AABB2D); inline;
function volume (): Float; 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 (const aabb: AABB2D): Boolean; inline; overload;
function contains (ax, ay: Float): 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: PFloat=nil; tmaxo: PFloat=nil): Boolean; overload;
+ function intersects (ax, ay, bx, by: Float): Boolean; inline; overload;
property valid: Boolean read getvalid;
property centerX: Float read getcenterX;
* 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
// a node is either a leaf (has data) or is an internal node (has children)
children: array [0..1] of Integer; // left and right child of the node (children[0] = left child)
//TODO: `flesh` can be united with `children`
- //flesh: Integer;
+ flesh: TTreeFlesh;
// height of the node in the tree (-1 for free nodes)
height: SmallInt;
// fat axis aligned bounding box (AABB) corresponding to the node
aabb: AABB2D;
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];
+ //property flesh: Integer read children[0] write children[0];
end;
TVisitCheckerCB = function (node: PTreeNode): Boolean is nested;
- TVisitVisitorCB = function (abody: Integer): Boolean is nested;
+ TVisitVisitorCB = function (abody: TTreeFlesh): Boolean is nested;
public
// return `true` to stop
- type TForEachLeafCB = function (abody: Integer; const aabb: AABB2D): Boolean is nested; // WARNING! don't modify AABB here!
+ type TForEachLeafCB = function (abody: TTreeFlesh; const aabb: AABB2D): Boolean is nested; // WARNING! don't modify AABB here!
public
// 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);
+ const LinearMotionGapMultiplier = 1.7;
private
mNodes: array of TTreeNode; // nodes of the tree
public
// called when a overlapping node has been found during the call to forEachAABBOverlap()
// return `true` to stop
- type TQueryOverlapCB = function (abody: Integer): Boolean is nested;
- type TSegQueryCallback = function (abody: Integer; ax, ay, bx, by: Float): Float is nested; // return dist from (ax,ay) to abody
+ 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: Integer;
+ flesh: TTreeFlesh;
procedure reset (); inline;
function valid (): Boolean; inline;
procedure getRootAABB (var aabb: AABB2D);
function isValidId (id: Integer): Boolean; inline;
- function getNodeObjectId (nodeid: Integer): Integer; inline;
+ function getNodeObjectId (nodeid: Integer): TTreeFlesh; inline;
procedure getNodeFatAABB (var aabb: AABB2D; nodeid: Integer); inline;
// return `false` for invalid flesh
- function getFleshAABB (var aabb: AABB2D; flesh: Integer): Boolean; virtual; abstract;
+ function getFleshAABB (var aabb: AABB2D; flesh: TTreeFlesh): 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: Integer; staticObject: Boolean=false): Integer;
+ function insertObject (flesh: TTreeFlesh; staticObject: Boolean=false): Integer;
// remove an object from the tree
// WARNING: ids of removed objects can be reused on later insertions!
*)
function updateObject (nodeId: Integer; dispX, dispY: Float; forceReinsert: Boolean=false): Boolean;
- procedure aabbQuery (ax, ay, aw, ah: Float; cb: TQueryOverlapCB);
- function pointQuery (ax, ay: Float; cb: TQueryOverlapCB): Integer;
+ function aabbQuery (ax, ay, aw, ah: Float; cb: TQueryOverlapCB): Boolean;
+ function pointQuery (ax, ay: Float; cb: TQueryOverlapCB): TTreeFlesh;
function segmentQuery (var qr: TSegmentQueryResult; ax, ay, bx, by: Float; cb: TSegQueryCallback): Boolean;
function computeTreeHeight (): Integer; // compute the height of the tree
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: Float): Float; inline; begin if (a < b) then result := a else result := b; end;
+function maxF (a, b: Float): Float; 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: 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 (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;
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: Float; aangle: Float); inline;
begin
origX := ax;
origY := ay;
dirY := sin(aangle);
end;
-procedure Ray2D.setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Float);
+procedure Ray2D.setX0Y0X1Y1 (ax0, ay0, ax1, ay1: Float); inline;
begin
origX := ax0;
origY := ay0;
// ////////////////////////////////////////////////////////////////////////// //
-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: Float); 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;
+
+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;
+
-procedure AABB2D.setXYWH (ax, ay, aw, ah: Float);
+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: Float); 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 (): Float; 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
end;
-function AABB2D.contains (ax, ay: Float): Boolean; overload;
+function AABB2D.contains (ax, ay: Float): 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
// 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: PFloat=nil; tmaxo: PFloat=nil): Boolean; overload;
var
dinv, t1, t2, tmp: Float;
tmin, tmax: Float;
// 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;
// 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
end;
end;
-function AABB2D.intersects (ax, ay, bx, by: Float): Boolean; overload;
+function AABB2D.intersects (ax, ay, bx, by: Float): Boolean; inline; overload;
var
tmin: Float;
ray: Ray2D;
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;
// ////////////////////////////////////////////////////////////////////////// //
-procedure TDynAABBTree.TSegmentQueryResult.reset (); begin dist := -1; flesh := -1; end;
-function TDynAABBTree.TSegmentQueryResult.valid (): Boolean; begin result := (dist >= 0) and (flesh >= 0); 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: Integer;
+ flesh := nil;
height := 0;
- aabb.setX0Y0X1Y1(0, 0, 0, 0);
+ aabb.minX := 0;
+ aabb.minY := 0;
+ aabb.maxX := 0;
+ aabb.maxY := 0;
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;
{$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;
{$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
// 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.0*mergedVolume;
// compute the minimum cost of pushing the new node further down the tree (inheritance cost)
- costI := Float(2.0)*(mergedVolume-volumeAABB);
+ costI := 2.0*(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;
balanceFactor := nodeC.height-nodeB.height;
// if the right node C is 2 higher than left node B
- if (balanceFactor > 1) then
+ if (balanceFactor > 1.0) then
begin
{$IFDEF aabbtree_many_asserts}assert(not nodeC.leaf);{$ENDIF}
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
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;
// get the children nodes
pNode := @mNodes[nodeId];
assert(pNode.height >= 0);
+ if (not pNode.aabb.valid) then
+ begin
+ 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);
+ if pNode.leaf then
+ begin
+ getFleshAABB(aabb, pNode.flesh);
+ 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);
+ 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
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);
end;
begin
- result := false;
- if not assigned(dg) then exit;
// recursively check each node
result := forEachNode(mRootNodeId);
end;
// get object by nodeid; can return nil for invalid ids
-function TDynAABBTree.getNodeObjectId (nodeid: Integer): Integer;
+function TDynAABBTree.getNodeObjectId (nodeid: Integer): TTreeFlesh;
begin
- if (nodeid >= 0) and (nodeid < mNodeCount) and (mNodes[nodeid].leaf) then result := mNodes[nodeid].flesh else result := -1;
+ if (nodeid >= 0) and (nodeid < mNodeCount) and (mNodes[nodeid].leaf) then result := mNodes[nodeid].flesh else result := nil;
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;
// 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: Integer; staticObject: Boolean=false): Integer;
+function TDynAABBTree.insertObject (flesh: TTreeFlesh; 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
+ 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);
+ //raise Exception.Create('trying to insert invalid flesh in dyntree');
+ result := -1;
+ exit;
+ end;
+ if not aabb.valid then
+ begin
+ 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);
+ 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;
@@ -1269,9 +1306,10 @@ function TDynAABBTree.updateObject (nodeId: Integer; dispX, dispY: Float; forceR
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;
// 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;
// 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: Float; cb: TQueryOverlapCB): Boolean;
var
caabb: AABB2D;
function checker (node: PTreeNode): Boolean;
end;
begin
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);
+ result := (visit(checker, cb) <> -1);
end;
-// report body that contains the given point, or -1
-function TDynAABBTree.pointQuery (ax, ay: Float; cb: TQueryOverlapCB): Integer;
+// 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
nid := visit(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 := -1;
+ if (nid >= 0) then result := mNodes[nid].flesh else result := nil;
end;
result := node.aabb.intersects(curax, curay, curbx, curby);
end;
- function visitor (flesh: Integer): Boolean;
+ function visitor (flesh: TTreeFlesh): Boolean;
var
hitFraction: Float;
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;