diff --git a/Project.toml b/Project.toml index cbeabea..e0c854a 100644 --- a/Project.toml +++ b/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" diff --git a/src/CollisionDetection.jl b/src/CollisionDetection.jl index ed2c646..75ffaf6 100644 --- a/src/CollisionDetection.jl +++ b/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") diff --git a/src/octree.jl b/src/octree.jl index a741941..ef47b62 100644 --- a/src/octree.jl +++ b/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,9 +96,6 @@ 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) @@ -98,12 +103,26 @@ function Octree(points::Vector, radii::Vector{T}, expanding_ratio=1.0, splitcoun # 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) diff --git a/src/searcheq.jl b/src/searcheq.jl index 1f11780..21fd9ac 100644 --- a/src/searcheq.jl +++ b/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 diff --git a/test/Project.toml b/test/Project.toml index b4f2d65..07c9bb6 100644 --- a/test/Project.toml +++ b/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" diff --git a/test/REQUIRE b/test/REQUIRE deleted file mode 100644 index f4c3b38..0000000 --- a/test/REQUIRE +++ /dev/null @@ -1,2 +0,0 @@ -StaticArrays 0.3.0 -JLD2 0.0.6 diff --git a/test/assets/mesh_bounds_100k.jld2 b/test/assets/mesh_bounds_100k.jld2 new file mode 100644 index 0000000..f29ea85 Binary files /dev/null and b/test/assets/mesh_bounds_100k.jld2 differ diff --git a/test/runtests.jl b/test/runtests.jl index 4d07280..aa81bff 100644 --- a/test/runtests.jl +++ b/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 diff --git a/test/test_regressions.jl b/test/test_regressions.jl new file mode 100644 index 0000000..7a9320d --- /dev/null +++ b/test/test_regressions.jl @@ -0,0 +1,261 @@ +using Test +using Random +using StaticArrays + +function stored_ids(tree) + ids = Int[] + for box in CD.boxes(tree) + append!(ids, box) + end + ids +end + +@testset "octree primitive boundaries" begin + c = @SVector [0.0, 0.0, 0.0] + @test CD.boxesoverlap(c, 1.0, @SVector([2.0, 0.0, 0.0]), 1.0) + @test !CD.boxesoverlap(c, 1.0, @SVector([2.1, 0.0, 0.0]), 1.0) + @test CD.fitsinbox(@SVector([0.0, 0.0]), 1.0, @SVector([0.0, 0.0]), 1.0) + @test !CD.fitsinbox(@SVector([0.0, 0.0]), 1.01, @SVector([0.0, 0.0]), 1.0) + @test CD.fitsinbox(@SVector([0.0, 0.0]), 1.1, @SVector([0.0, 0.0]), 1.0, 1.1) + @test CD.childcentersize(@SVector([0.0, 0.0]), 2.0, 3) == + (@SVector([1.0, 1.0]), 1.0) +end + +@testset "octree storage invariants" begin + points = [@SVector([Float64(x), Float64(y), Float64(z)]) + for x in -2:2, y in -2:2, z in -2:2] + points = vec(points) + radii = [isodd(i) ? 0.0 : 0.15 for i in eachindex(points)] + tree = CD.Octree(points, radii, 1.1, 4) + ids = stored_ids(tree) + @test length(tree) == length(points) + @test sort(ids) == collect(1:length(points)) + + # Coincident objects cannot be separated by spatial subdivision. They + # must remain in a box, once each, rather than causing unbounded splitting. + coincident = CD.Octree(fill(@SVector([0.0, 0.0]), 32), zeros(32), 1.1, 4) + @test sort(stored_ids(coincident)) == collect(1:32) +end + +@testset "object bounds are respected during subdivision" begin + points = [@SVector([0.0, 0.0]), @SVector([1.0, 0.0])] + radii = [0.6, 0.0] + tree = CD.Octree(points, radii, 1.1, 1) + + # The first object's bounding box protrudes across the child boundary. It must + # remain searchable even though its center belongs to only one child. + found = collect(CD.searchtree(_ -> true, tree, (@SVector([0.5, 0.0]), 0.05))) + @test found == [1] +end + +@testset "search APIs agree with brute force" begin + points = [@SVector([Float64(x), Float64(y), Float64(z)]) + for x in -4:4, y in -3:3, z in -2:2] + points = vec(points) + radii = [0.05 + 0.02 * mod(i, 4) for i in eachindex(points)] + tree = CD.Octree(points, radii, 1.1, 6) + + queries = ( + (@SVector([0.0, 0.0, 0.0]), 0.4), + (@SVector([2.0, -1.0, 0.5]), 0.75), + (@SVector([20.0, 20.0, 20.0]), 0.1), + ) + + for (center, halfsize) in queries + bb = (center, halfsize) + expected = [i for i in eachindex(points) + if CD._overlapsbounds(points[i] .- radii[i], points[i] .+ radii[i], + center .- halfsize, center .+ halfsize)] + pred(i) = iseven(i) || points[i][1] < 0 + expected = filter(pred, expected) + + found = collect(CD.searchtree(pred, tree, bb)) + visited = Int[] + CD.foreachsearchtree(tree, bb) do i + pred(i) && push!(visited, i) + false + end + @test sort(found) == sort(expected) + @test sort(visited) == sort(expected) + @test CD.anysearchtree(pred, tree, bb) == !isempty(expected) + end +end + +@testset "search visitor early termination" begin + points = [@SVector([Float64(x), 0.0]) for x in 1:20] + tree = CD.Octree(points, zeros(20), 1.1, 3) + seen = Ref(0) + stopped = CD.foreachsearchtree(tree, (@SVector([10.0, 0.0]), 20.0)) do _ + seen[] += 1 + true + end + @test stopped + @test seen[] == 1 +end + +function brute_force_ids(points, radii, center, halfsize, pred) + querylower = center .- halfsize + queryupper = center .+ halfsize + [i for i in eachindex(points) + if CD._overlapsbounds( + points[i] .- radii[i], + points[i] .+ radii[i], + querylower, + queryupper, + ) && pred(i)] +end + +@testset "randomized search agrees with brute force" begin + rng = MersenneTwister(0xC0111D) + + for dimension in (2, 3) + point_type = dimension == 2 ? SVector{2,Float64} : SVector{3,Float64} + points = [point_type(rand(rng, dimension) .* 2 .- 1) for _ in 1:1000] + radii = rand(rng, 1000) .* 0.08 + + # Include cases that random sampling rarely produces reliably. + append!(points, [zero(point_type), zero(point_type), point_type(ones(dimension))]) + append!(radii, [0.0, 0.25, 0.0]) + + for (splitcount, expansion_ratio) in ((4, 1.0), (8, 1.1), (16, 1.25)) + tree = CD.Octree(points, radii, expansion_ratio, splitcount) + for _ in 1:40 + center = point_type(rand(rng, dimension) .* 3 .- 1.5) + halfsize = rand(rng) .* 0.5 + bb = (center, halfsize) + pred = i -> iszero(mod(i, 5)) || points[i][1] < -0.2 + expected = brute_force_ids(points, radii, center, halfsize, pred) + + found = collect(CD.searchtree(pred, tree, bb)) + visited = Int[] + CD.foreachsearchtree(tree, bb) do id + pred(id) && push!(visited, id) + false + end + + @test sort(found) == sort(expected) + @test sort(visited) == sort(expected) + @test CD.anysearchtree(pred, tree, bb) == !isempty(expected) + end + end + end +end + +@testset "Float32 search agrees with brute force" begin + rng = MersenneTwister(0xF032) + point_type = SVector{3,Float32} + points = [point_type(rand(rng, Float32, 3) .* 2f0 .- 1f0) + for _ in 1:3000] + radii = rand(rng, Float32, 3000) .* 0.08f0 + tree = CD.Octree(points, radii, 1.1, 16) + + for _ in 1:300 + center = point_type(rand(rng, Float32, 3) .* 3f0 .- 1.5f0) + halfsize = rand(rng, Float32) * 0.5f0 + expected = brute_force_ids(points, radii, center, halfsize, _ -> true) + found = collect(CD.searchtree(_ -> true, tree, (center, halfsize))) + @test sort(found) == sort(expected) + end +end + +@testset "targeted query bounds" begin + points = [ + @SVector([-1.0, -1.0, -1.0]), + @SVector([1.0, 1.0, 1.0]), + @SVector([0.0, 0.0, 0.0]), + ] + radii = [0.0, 0.0, 0.5] + tree = CD.Octree(points, radii, 1.1, 1) + + queries = ( + (@SVector([0.0, 0.0, 0.0]), 0.0), + (@SVector([0.5, 0.5, 0.5]), 0.0), + (@SVector([0.0, 0.0, 0.0]), 0.5), + (@SVector([10.0, 10.0, 10.0]), 0.0), + ) + for (center, halfsize) in queries + expected = brute_force_ids(points, radii, center, halfsize, _ -> true) + @test sort(collect(CD.searchtree(_ -> true, tree, (center, halfsize)))) == + sort(expected) + end +end + +@testset "default tree ratio preserves raw box queries" begin + points = [@SVector([Float64(x), Float64(y)]) + for x in 0:9, y in 0:9] + points = vec(points) + radii = fill(0.4, length(points)) + tree = CD.Octree(points, radii) + + for id in eachindex(points) + center = points[id] + radius = radii[id] + found = Int[] + predicate = (box_center, box_halfsize) -> + CD.fitsinbox(center, radius, box_center, box_halfsize + 1e-12) + for box in CD.boxes(tree, predicate) + append!(found, box) + end + @test id in found + end +end + +@testset "two-dimensional minimum box size uses dimensional scaling" begin + points = [@SVector([0.0, 0.0]), @SVector([2.0, 0.0]), + @SVector([0.0, 2.0]), @SVector([2.0, 2.0])] + splitcount = 8 + tree = CD.Octree(points, zeros(4), 1.0, splitcount) + expected = 0.1 * tree.halfsize * (splitcount / length(points))^(1 / 2) + old_three_dimensional_formula = + 0.1 * tree.halfsize * (splitcount / length(points))^(1 / 3) + @test tree.minhalfsize ≈ expected + @test tree.minhalfsize != old_three_dimensional_formula +end + +@testset "mesh-derived bounds agree with brute force" begin + fixture = joinpath(@__DIR__, "assets", "mesh_bounds_100k.jld2") + data = JLD2.load(fixture) + rng = MersenneTwister(0x5EEDDA7A) + + cases = ( + (data["circle_centers"], data["circle_radii"]), + (data["sphere_centers"], data["sphere_radii"]), + ) + for (center_data, radii) in cases + dimension = size(center_data, 1) + point_type = dimension == 2 ? SVector{2,Float64} : SVector{3,Float64} + points = [point_type(center_data[:, i]) for i in axes(center_data, 2)] + queries = [ + (zero(point_type), 0.0), + (zero(point_type), 0.01), + (zero(point_type), 0.1), + (zero(point_type), 0.5), + (point_type(fill(2.0, dimension)), 0.01), + ] + append!(queries, [ + (point_type(rand(rng, dimension) .* 3 .- 1.5), rand(rng) * 0.2) + for _ in 1:995 + ]) + + for (splitcount, expansion_ratio) in ((4, 1.0), (16, 1.1), (64, 1.25)) + tree = CD.Octree(points, radii, expansion_ratio, splitcount) + for (center, halfsize) in queries + bb = (center, halfsize) + pred = i -> iseven(i) || points[i][1] < 0 + expected = brute_force_ids(points, radii, center, halfsize, pred) + found = collect(CD.searchtree(pred, tree, bb)) + visited = Int[] + CD.foreachsearchtree(tree, bb) do id + pred(id) && push!(visited, id) + false + end + + @test sort(found) == sort(expected) + @test sort(visited) == sort(expected) + @test length(unique(found)) == length(found) + @test length(unique(visited)) == length(visited) + @test CD.anysearchtree(pred, tree, bb) == !isempty(expected) + end + end + end +end diff --git a/test/test_searcheq.jl b/test/test_searcheq.jl index 7688c84..db04e33 100644 --- a/test/test_searcheq.jl +++ b/test/test_searcheq.jl @@ -29,4 +29,4 @@ ids = collect(CD.searchtree(pred, tree, bb)) ids2 = findall(i -> all(ctrs[i].+rads[i] .> 0), 1:N) @test length(ids2) == length(ids) @test sort(ids2) == sort(ids) -@test ids == [26, 46, 54, 93, 34, 94, 75, 23, 86, 57, 44, 40, 67, 73, 77, 80] +@test sort(ids) == sort([26, 46, 54, 93, 34, 94, 75, 23, 86, 57, 44, 40, 67, 73, 77, 80])