Add foreachsearchtree/anysearchtree and cache object bounds; bump to …

Project.toml

@@ -1,6 +1,6 @@
 name = "CollisionDetection"
 uuid = "2b5bf9a6-f3f8-5352-af9c-82bb4af718d8"
-version = "0.1.8"
+version = "0.2.0"
 
 [deps]
 Compat = "34da2185-b29b-5c13-b0c7-acf172513d20"

src/CollisionDetection.jl

@@ -8,6 +8,7 @@ using StaticArrays
 export Octree
 export boxes, fitsinbox, boudingbox, boxesoverlap
 export searchtree, find
+export foreachsearchtree, anysearchtree
 
 
 include("octree.jl")

src/octree.jl

@@ -1,4 +1,3 @@
-
 mutable struct Box
     data::Vector{Int}
     children::Vector{Box}
@@ -17,6 +16,8 @@ mutable struct Octree{T,P}
 
     points::Vector{P}
     radii::Vector{T}
+    lowers::Vector{P} # Read-only cached lower bounds of each object's bounding box.
+    uppers::Vector{P} # Read-only cached upper bounds of each object's bounding box.
     expanding_ratio::Float64 # This will add the expanding ratio of boxes
 
     splitcount::Int
@@ -68,8 +69,15 @@ function boxesoverlap(c1, hs1, c2, hs2)
     return true
 end
 
-function Octree(points::Vector, radii::Vector{T}, expanding_ratio=1.0, splitcount = 10,  minhalfsize = zero(T)) where {T}
+"""
+    Octree(points, radii, expanding_ratio=1.0, splitcount=10, minhalfsize=0)
 
+Build an octree for object centers and radii. The default expansion ratio is
+one. Callers using the raw `boxes(tree, predicate)` iterator must account for
+`tree.expanding_ratio` in their predicate when constructing a tree with a
+larger ratio; the iterator itself does not expand predicate arguments.
+"""
+function Octree(points::Vector, radii::Vector{T}, expanding_ratio=1.0, splitcount = 10,  minhalfsize = zero(T)) where {T}
     n_points = length(points)
     n_dims = length(eltype(points))
 
@@ -88,22 +96,33 @@ function Octree(points::Vector, radii::Vector{T}, expanding_ratio=1.0, splitcoun
 	ll = ll .- radius
 	ur = ur .+ radius
 
-    #ll = minimum(points) - radius
-    #ur = maximum(points) + radius
-
     center = (ll + ur) / 2
     halfsize = maximum(ur - center)
 
     # if the minimal box size is not specified,
     # make a reasonable guess
     if minhalfsize == 0
         #TODO generalise
-        minhalfsize = T( 0.1 * halfsize * (splitcount / n_points)^(1/3))
+        minhalfsize = T(0.1 * halfsize * (splitcount / n_points)^(1 / n_dims))
     end
 
+    lowers = [point .- radius for (point, radius) in zip(points, radii)]
+    uppers = [point .+ radius for (point, radius) in zip(points, radii)]
+
     # Create an empty octree
     rootbox = Box()
-    tree = Octree(center, halfsize, rootbox, points, radii, expanding_ratio, splitcount, minhalfsize)
+    tree = Octree(
+        center,
+        halfsize,
+        rootbox,
+        points,
+        radii,
+        lowers,
+        uppers,
+        expanding_ratio,
+        splitcount,
+        minhalfsize,
+    )
 
     # populate
     for id in 1:n_points
@@ -165,6 +184,21 @@ function fitsinbox(pos, radius, center, halfsize)
 # the code judege by comapring with the box lower left point and uper right point
 	return true
 end
+
+"""
+    fitsboundsinbox(lower, upper, center, halfsize, ratio) -> Bool
+
+Return `true` when the axis-aligned bounds `lower` and `upper` fit inside
+the box described by `center` and `halfsize`, after expanding the box by
+the multiplicative factor `ratio`.
+"""
+function fitsboundsinbox(lower, upper, center, halfsize, ratio)
+    for i in eachindex(center)
+        lower[i] < center[i] - halfsize * ratio && return false
+        upper[i] > center[i] + halfsize * ratio && return false
+    end
+    return true
+end
 """
   itsinbox(Opj_c, Obj_rad, box_c, box_hf, ratio) -> true/fasle
 
@@ -255,7 +289,9 @@ function insert!(tree, box, center::P, halfsize::T, point::P, radius::T, id) whe
         sct = childsector(point, center)
         chdbox = box.children[sct+1]
         chdcenter, chdhalfsize = childcentersize(center, halfsize, sct)
-        if fitsinbox(point, radius, chdcenter, chdhalfsize,expanding_ratio) # check if it is fat and please consider the expansion
+        if fitsboundsinbox(
+            tree.lowers[id], tree.uppers[id], chdcenter, chdhalfsize,expanding_ratio
+        )
           insert!(tree, chdbox, chdcenter, chdhalfsize, point, radius, id)
         else
           push!(box.data, id)
@@ -300,7 +336,9 @@ function insert!(tree, box, center::P, halfsize::T, point::P, radius::T, id) whe
             sct = childsector(point, center)
             chdbox = box.children[sct+1]
             chdcenter, chdhalfsize = childcentersize(center, halfsize, sct)
-            if fitsinbox(point, radius, chdcenter, chdhalfsize,expanding_ratio)# # check if it is fat for the child and please consider the expansion
+            if fitsboundsinbox(
+                tree.lowers[id], tree.uppers[id], chdcenter, chdhalfsize, expanding_ratio
+            )
                 push!(chdbox.data, id)
             else
                 push!(unmovables, id)
@@ -394,6 +432,13 @@ mutable struct BoxIteratorStage{T,P}
     halfsize::T
 end
 
+"""
+    boxes(tree, predicate)
+
+Iterate over boxes accepted by `predicate`. Raw predicates receive the nominal
+box center and halfsize, so they must account for `tree.expanding_ratio`
+themselves when a tree was built with an expansion ratio larger than one.
+"""
 boxes(tree::Octree, pred = (ctr,hsz)->true) = BoxIterator(pred, tree)
 function boxes(tree::Octree, center, halfsize)
     pred = (c,s) -> boxesoverlap(c, s * tree.expanding_ratio, center, halfsize)

src/searcheq.jl

@@ -1,13 +1,132 @@
+@inline function _overlapsbounds(lower, upper, querylower, queryupper)
+    @inbounds for d in eachindex(querylower)
+        lower[d] <= queryupper[d] || return false
+        upper[d] >= querylower[d] || return false
+    end
+    return true
+end
+
 """
     searchtree(pred, tree, bb)
 
-Return an iterator that returns indices stores in tree that correspond to objects for which the supplied predicate holds.
+Return an iterator over indices stored in `tree` for which the supplied
+predicate holds.
+
+`pred` receives an integer index and returns `true` when the corresponding
+object should be included. `bb` is the query bounding box; it is used to
+exclude non-overlapping branches and object bounds before evaluating `pred`.
 
-`pred` takes an integer an returns true is the corresponding object stored in `tree` collides with the target object. `bb` is a bounding box containing the target. This bounding box is used for effiently excluding entire branches of the tree.
+The query bounds are a filter, not merely a hint.
 """
 function searchtree(pred, tree::Octree, bb)
     ct, st = bb
     box_pred = (c,s) ->  boxesoverlap(c, s * tree.expanding_ratio, ct, st)
     box_it = boxes(tree, box_pred)
-    Compat.Iterators.filter(pred, Compat.Iterators.flatten(box_it))
+    query_lower = ct .- st
+    query_upper = ct .+ st
+    candidate_it = Compat.Iterators.flatten(box_it)
+    Compat.Iterators.filter(candidate_it) do id
+        _overlapsbounds(tree.lowers[id], tree.uppers[id], query_lower, query_upper) &&
+            pred(id)
+    end
+end
+
+struct SearchTreeVisitor{T,P,F,C,S,L,U}
+    tree::T
+    predicate::P
+    callback::F
+    querycenter::C
+    queryst::S
+    querylower::L
+    queryupper::U
+end
+
+function (visitor::SearchTreeVisitor)(id)
+    visitor.predicate(id) || return false
+    return visitor.callback(id) === true
+end
+
+struct CombinedSearchTreeVisitor{T,F,C,S,L,U}
+    tree::T
+    callback::F
+    querycenter::C
+    queryst::S
+    querylower::L
+    queryupper::U
+end
+
+function (visitor::CombinedSearchTreeVisitor)(id)
+    return visitor.callback(id) === true
+end
+
+function _visitsearchtree!(visitor::V, box, center, halfsize) where {V}
+    boxesoverlap(
+        center,
+        halfsize * visitor.tree.expanding_ratio,
+        visitor.querycenter,
+        visitor.queryst,
+    ) || return false
+
+    for id in box.data
+        _overlapsbounds(
+            visitor.tree.lowers[id],
+            visitor.tree.uppers[id],
+            visitor.querylower,
+            visitor.queryupper,
+        ) && visitor(id) && return true
+    end
+
+    for sector in 0:(length(box.children) - 1)
+        childcenter, childhalfsize = childcentersize(center, halfsize, sector)
+        _visitsearchtree!(
+            visitor,
+            box.children[sector + 1],
+            childcenter,
+            childhalfsize,
+        ) && return true
+    end
+    return false
+end
+
+"""
+    foreachsearchtree(f, tree, bb, pred) -> stopped
+
+Apply `f` to every stored object whose bounds intersect `bb` and for which
+`pred` returns `true`. Return `true` when `f` returns `true` and traversal
+stops; otherwise return `false` after exhausting the search.
+
+The callback must return the singleton `true` to stop traversal; other truthy
+values are treated as `false`. With a `do` block, the callback comes first:
+`foreachsearchtree(tree, bb, pred) do id ... end`.
+"""
+function foreachsearchtree(f, tree::Octree, bb, pred)
+    ct, st = bb
+    visitor = SearchTreeVisitor(tree, pred, f, ct, st, ct .- st, ct .+ st)
+    return _visitsearchtree!(visitor, tree.rootbox, tree.center, tree.halfsize)
+end
+
+"""
+    foreachsearchtree(visitor, tree, bb) -> stopped
+
+Apply `visitor` to every stored object whose bounds intersect `bb`.
+The visitor may perform object-level filtering and processing; return the
+singleton `true` to stop traversal early. Other truthy values are treated as
+`false`. Return `false` when traversal is exhausted.
+"""
+function foreachsearchtree(visitor, tree::Octree, bb)
+    ct, st = bb
+    state = CombinedSearchTreeVisitor(tree, visitor, ct, st, ct .- st, ct .+ st)
+    return _visitsearchtree!(state, tree.rootbox, tree.center, tree.halfsize)
+end
+
+"""
+    anysearchtree(pred, tree, bb) -> Bool
+
+Return `true` when at least one stored object whose bounds intersect `bb`
+is accepted by `pred`. Stop traversal after the first accepted object.
+"""
+function anysearchtree(pred, tree::Octree, bb)
+    return foreachsearchtree(tree, bb, pred) do _
+        true
+    end
 end

test/Project.toml

@@ -1,8 +1,8 @@
 [deps]
-Compat = "34da2185-b29b-5c13-b0c7-acf172513d20"
 DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab"
 JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 

test/runtests.jl

@@ -11,5 +11,6 @@ import CollisionDetection
 include("test_core.jl")
 include("test_bloated.jl")
 include("test_searcheq.jl")
+include("test_regressions.jl")
 
 end # module