<<>> <> <> <> <> DIRECTORY Basics, QuadTree, Real, Vector, IO, FS, Rope; QuadTreeImpl: CEDAR MONITOR LOCKS tree USING tree: QT IMPORTS Basics, Real, IO, FS EXPORTS QuadTree = BEGIN OPEN Basics, QuadTree; testing: BOOLEAN = FALSE; QT: TYPE = REF QTRec; QTRec: PUBLIC TYPE = MONITORED RECORD [ structure: QTNode ¬ NIL, universe: BoundingBox ]; QTNode: TYPE = REF QTNodeRec; QTNodeRec: TYPE = RECORD [ count: INTEGER ¬ 0, --set to -1 once the node is split level: CARDINAL, area: InternalBoundingBox, items: LIST OF QTItem ¬ NIL, scan: LIST OF QTItem ¬ NIL, --used to scan items during enumerate quadrants: ARRAY[0..3] OF QTNode ¬ ALL[NIL] ]; splitThreshold: INTEGER = 18; maxLevels: CARDINAL = 15; Create: PUBLIC PROC [universe: BoundingBox] RETURNS [tree:QT] ~ { structure: QTNode ¬ NEW[QTNodeRec ¬ [ area: [ [FIRST[CARDINAL], FIRST[CARDINAL]], [LAST[CARDINAL], LAST[CARDINAL]] ], level: 0 ]]; RETURN[ NEW[QTRec ¬ [structure: structure, universe: universe]] ]; }; Insert: PUBLIC PROC [tree: QT, itemSize: BoundingBox, itemData: REF ANY, locked: BOOL ¬ FALSE] ~ { item: QTItem ¬ NEW[QTItemRec ¬ [size: IntFromExt[itemSize, tree.universe], data: itemData]]; root: QTNode ¬ tree.structure; IF locked THEN LockedInsert[tree, item] ELSE InsertItemInNode[item, root]; }; LockedInsert: PROC [tree: QT, item: QTItem] ~ { root: QTNode ¬ tree.structure; InsertItemInNode[item, root]; }; IntFromExt: PROC [ext, universe: BoundingBox] RETURNS [int: InternalBoundingBox] ~ { int.lowerLeft ¬ ScaleVec[ext.lowerLeft, universe]; int.upperRight ¬ ScaleVec[ext.upperRight, universe]; }; ScaleVec: PROC [v: Vector.VEC, u: BoundingBox] RETURNS [iv: InternalVEC] ~ { iv.x ¬ ScaleInRange[v.x, u.lowerLeft.x, u.upperRight.x]; iv.y ¬ ScaleInRange[v.y, u.lowerLeft.y, u.upperRight.y]; }; ScaleInRange: PROC [r, l, u: REAL] RETURNS [i: CARDINAL] ~ INLINE{ IF (r <= l) THEN RETURN[0]; IF (r >= u) THEN RETURN[65535]; RETURN[Real.Fix[( (r - l) * 65535.0 ) / (u - l)]]; }; InsertItemInNode: PROC [item: QTItem, node: QTNode] ~ { IF node.level = maxLevels THEN node.items ¬ CONS[item, node.items] ELSE IF node.count >= 0 THEN { node.items ¬ CONS[item, node.items]; node.count ¬ node.count + 1; IF node.count > splitThreshold THEN { node.count ¬ -1; node.scan ¬ node.items; node.items ¬ NIL; UNTIL node.scan = NIL DO InsertItemInNode[node.scan.first, node]; node.scan ¬ node.scan.rest; ENDLOOP; }; } ELSE { -- node.count = -1 means node is split already quad: INTEGER ¬ LocateQuadrant[item, node.level]; IF quad = -1 THEN node.items ¬ CONS[item, node.items] --belongs here ELSE { IF node.quadrants[quad] = NIL THEN node.quadrants[quad] ¬ NewNode[node, quad]; InsertItemInNode[item, node.quadrants[quad]]; }; }; }; LocateQuadrant: PROC [item: QTItem, level: CARDINAL] RETURNS [quad: INTEGER] ~ { xBit: CARDINAL ¬ ExtractBit[item.size.lowerLeft.x, 15 - level]; yBit: CARDINAL ¬ ExtractBit[item.size.lowerLeft.y, 15 - level]; quadLL: CARDINAL ¬ xBit + 2*yBit; quadUR: CARDINAL; xBit ¬ ExtractBit[item.size.upperRight.x, 15 - level]; yBit ¬ ExtractBit[item.size.upperRight.y, 15 - level]; quadUR ¬ xBit + 2*yBit; IF quadLL = quadUR THEN RETURN[quadLL] ELSE RETURN[-1]; }; ExtractBit: PROC [w: CARDINAL, loc: CARDINAL] RETURNS [b: CARDINAL] ~ INLINE { RETURN[BITAND[BITSHIFT[LOOPHOLE[w], -loc], 1]]; }; NewNode: PROC [parent: QTNode, quad: INTEGER] RETURNS [son: QTNode] ~ { son ¬ NEW[QTNodeRec ¬ [ level: parent.level + 1, area: SubArea[parent.area, parent.level, quad] ]]; }; SubArea: PROC [org: InternalBoundingBox, level: CARDINAL, quad: INTEGER] RETURNS [sub: InternalBoundingBox] ~ { yBit: CARDINAL ¬ quad/2; xBit: CARDINAL ¬ quad - 2*yBit; sub.lowerLeft.x ¬ ComputeCorner[org.lowerLeft.x, xBit, level, 0]; sub.lowerLeft.y ¬ ComputeCorner[org.lowerLeft.y, yBit, level, 0]; sub.upperRight.x ¬ ComputeCorner[org.lowerLeft.x, xBit, level, 1]; sub.upperRight.y ¬ ComputeCorner[org.lowerLeft.y, yBit, level, 1]; }; ComputeCorner: PROC [base: CARDINAL, bit: CARDINAL, level: CARDINAL, pad: CARDINAL] RETURNS [result: CARDINAL] ~ { scratch: WORD ¬ bit + pad; result ¬ BITOR[ LOOPHOLE[base], BITSHIFT[scratch, 15 - level] - pad ]; }; Enumerate: PUBLIC PROC [tree: QT, region: BoundingBox, PerItem: PerItemProc, quanta: CARD ¬ LAST[CARD], locked: BOOL ¬ FALSE ] ~ { IF quanta < LAST[CARD] AND NOT locked THEN ERROR; IF locked THEN LockedEnumerate[tree, region, PerItem, quanta] ELSE InnerEnumerate[tree, region, PerItem, quanta]; }; LockedEnumerate: ENTRY PROC [tree: QT, region: BoundingBox, PerItem: PerItemProc, quanta: CARD ¬ LAST[CARD] ] ~ { ENABLE UNWIND => NULL; InnerEnumerate[tree, region, PerItem, quanta]; }; InnerEnumerate: PROC [tree: QT, region: BoundingBox, PerItem: PerItemProc, quanta: CARD ¬ LAST[CARD] ] ~ { intRegion: InternalBoundingBox ¬ IntFromExt[region, tree.universe]; ResetScans: PerNodeProc ~ { node.scan ¬ node.items; }; extraPassNeeded: BOOL ¬ TRUE; ProcessAQuanta: PerNodeProc ~ { quantaCount: CARD ¬ 0; UNTIL node.scan = NIL DO IF quantaCount >= quanta THEN { extraPassNeeded ¬ TRUE; RETURN[FALSE]; --this node finished for now }; IF Intersect[region, node.scan.first.size].overlap THEN { IF PerItem[node.scan.first] THEN RETURN[TRUE]; --quit requested quantaCount ¬ quantaCount + 1; }; node.scan ¬ node.scan.rest; ENDLOOP; RETURN[FALSE]; --this node finished }; ProcessAll: PerNodeProc ~ { scan: LIST OF QTItem ¬ node.items; UNTIL scan = NIL DO IF Intersect[region, scan.first.size].overlap THEN IF PerItem[scan.first] THEN RETURN[TRUE]; --quit requested scan ¬ scan.rest; ENDLOOP; }; IF quanta < LAST[CARD] THEN TreeWalk[tree.structure, intRegion, ResetScans]; UNTIL extraPassNeeded = FALSE DO extraPassNeeded ¬ FALSE; TreeWalk[tree.structure, intRegion, IF quanta < LAST[CARD] THEN ProcessAQuanta ELSE ProcessAll]; ENDLOOP; }; PerNodeProc: TYPE = PROC [node: QTNode, region: InternalBoundingBox] RETURNS [quit: BOOLEAN ¬ FALSE]; TreeWalk: PROC [node: QTNode, region: InternalBoundingBox, PerNode: PerNodeProc] ~ { IF PerNode[node, region] THEN RETURN; FOR quad: CARDINAL IN [0..3] DO nextNode: QTNode ¬ node.quadrants[quad]; nextRegion: InternalBoundingBox; lap: BOOLEAN; IF nextNode = NIL THEN LOOP; [nextRegion, lap] ¬ Intersect[region, nextNode.area]; IF lap THEN TreeWalk[nextNode, nextRegion, PerNode]; ENDLOOP; }; Intersect: PROC [b1, b2: InternalBoundingBox] RETURNS [common: InternalBoundingBox, overlap: BOOLEAN] ~ { common.lowerLeft.x ¬ MAX[b1.lowerLeft.x, b2.lowerLeft.x]; common.lowerLeft.y ¬ MAX[b1.lowerLeft.y, b2.lowerLeft.y]; common.upperRight.x ¬ MIN[b1.upperRight.x, b2.upperRight.x]; common.upperRight.y ¬ MIN[b1.upperRight.y, b2.upperRight.y]; overlap ¬ (common.lowerLeft.x <= common.upperRight.x) AND (common.lowerLeft.y <= common.upperRight.y); }; Destroy: PUBLIC ENTRY PROC [tree: QT, PerItem: PerItemProc ¬ NIL] ~ { ENABLE UNWIND => NULL; TreeDestroyWalk: PROC [node: QTNode] ~ { scan, temp: LIST OF QTItem; IF node = NIL THEN RETURN; scan ¬ node.items; UNTIL scan = NIL DO IF PerItem # NIL THEN [] ¬ PerItem[scan.first]; temp ¬ scan; scan ¬ scan.rest; temp.first ¬ NIL; temp.rest ¬ NIL; ENDLOOP; FOR quad: CARDINAL IN [0..3] DO nextNode: QTNode ¬ node.quadrants[quad]; [] ¬ TreeDestroyWalk[nextNode]; node.quadrants[quad] ¬ NIL; ENDLOOP; }; TreeDestroyWalk[tree.structure]; }; <> qt: QT; s: IO.STREAM; PrintItems: PerItemProc ~ { s.PutF1["QT item number %g\n", IO.int[NARROW[item.data, REF INT]­] ] }; IF testing THEN { s ¬ FS.StreamOpen["QuadTreeImpl.testlog", $create]; qt ¬ Create[[[0.0, 0.0], [10.0, 10.0]]]; Insert[qt, [[4.0, 4.0], [6.0, 6.0]], NEW[INT ¬ 1]]; Insert[qt, [[5.0, 2.0], [6.0, 3.0]], NEW[INT ¬ 2]]; Insert[qt, [[7.0, 7.0], [8.0, 8.0]], NEW[INT ¬ 3]]; Enumerate[qt, [[6.0, 6.0], [6.0, 6.0]], PrintItems]; Destroy[qt]; s.Close[]; }; END.