diff --git a/README.md b/README.md index 6fdfc47..bbcda29 100644 --- a/README.md +++ b/README.md @@ -82,6 +82,13 @@ bvh = BVH(bounding_spheres, BBox{Float32}, UInt32, 2) traversal = traverse(bvh, 3, traversal) ``` +Update previous BVH bounding volumes' positions and rebuild BVH *reusing previous memory*: + +```julia +new_positions = rand(3, 5) +bvh_rebuilt = BVH(bvh, new_positions) +``` + Compute contacts between two different BVH trees (e.g. two different robotic parts): ```julia diff --git a/docs/src/bounding_volumes.md b/docs/src/bounding_volumes.md index c1311a2..602d593 100644 --- a/docs/src/bounding_volumes.md +++ b/docs/src/bounding_volumes.md @@ -11,3 +11,9 @@ ImplicitBVH.BSphere ImplicitBVH.iscontact ImplicitBVH.center ``` + +## Miscellaneous + +```@docs +ImplicitBVH.translate +``` diff --git a/src/bounding_volumes.jl b/src/bounding_volumes.jl index 93eb230..ea15ef7 100644 --- a/src/bounding_volumes.jl +++ b/src/bounding_volumes.jl @@ -18,6 +18,15 @@ Get the coordinates of a bounding volume's centre, as a NTuple{3, T}. function center end +""" + translate(b::BSphere{T}, dx) where T + translate(b::BBox{T}, dx) where T + +Get a new bounding volume translated by dx; dx can be any iterable with 3 elements. +""" +function translate end + + """ $(TYPEDEF) @@ -154,6 +163,16 @@ end center(b::BSphere) = b.x +# Overloaded translate function +function translate(b::BSphere{T}, dx) where T + @assert length(dx) == 3 + new_center = (b.x[1] + T(dx[1]), + b.x[2] + T(dx[2]), + b.x[3] + T(dx[3])) + BSphere{T}(new_center, b.r) +end + + # Merge two bounding spheres function BSphere{T}(a::BSphere, b::BSphere) where T length = dist3(a.x, b.x) @@ -288,6 +307,20 @@ center(b::BBox{T}) where T = (T(0.5) * (b.lo[1] + b.up[1]), T(0.5) * (b.lo[3] + b.up[3])) +# Overloaded translate function +function translate(b::BBox{T}, dx) where T + @assert length(dx) == 3 + dx1, dx2, dx3 = T(dx[1]), T(dx[2]), T(dx[3]) + new_lo = (b.lo[1] + dx1, + b.lo[2] + dx2, + b.lo[3] + dx3) + new_up = (b.up[1] + dx1, + b.up[2] + dx2, + b.up[3] + dx3) + BBox{T}(new_lo, new_up) +end + + # Merge two bounding boxes function BBox{T}(a::BBox, b::BBox) where T lower = (minimum2(a.lo[1], b.lo[1]), diff --git a/src/build.jl b/src/build.jl index d0fbcec..98166df 100644 --- a/src/build.jl +++ b/src/build.jl @@ -32,10 +32,18 @@ Tree Level Nodes & Leaves Build Up Traverse Down num_threads=Threads.nthreads(), ) where {L, N, U <: MortonUnsigned} + function BVH( + prev::BVH, + new_positions::AbstractMatrix, + built_level=1; + num_threads=Threads.nthreads(), + ) + # Fields - `tree::`[`ImplicitTree`](@ref)`{Int}` - `nodes::VN <: AbstractVector` - `leaves::VL <: AbstractVector` +- `mortons::VM <: AbstractVector` - `order::VO <: AbstractVector` - `built_level::Int` @@ -104,18 +112,26 @@ cache: bvh = BVH(bounding_spheres, BBox{Float32}, UInt32, 2) traversal = traverse(bvh, 3, traversal) ``` + +Update previous BVH bounding volumes' positions and rebuild BVH *reusing previous memory*: + +```julia +new_positions = rand(3, 5) +bvh_rebuilt = BVH(bvh, new_positions) +``` """ -struct BVH{VN <: AbstractVector, VL <: AbstractVector, VO <: AbstractVector} +struct BVH{VN <: AbstractVector, VL <: AbstractVector, VM <: AbstractVector, VO <: AbstractVector} built_level::Int tree::ImplicitTree{Int} nodes::VN leaves::VL + mortons::VM order::VO end # Custom pretty-printing -function Base.show(io::IO, b::BVH{VN, VL, VO}) where {VN, VL, VO} +function Base.show(io::IO, b::BVH{VN, VL, VM, VO}) where {VN, VL, VM, VO} print( io, """ @@ -124,13 +140,14 @@ function Base.show(io::IO, b::BVH{VN, VL, VO}) where {VN, VL, VO} tree: $(b.tree) nodes: $(VN)($(size(b.nodes))) leaves: $(VL)($(size(b.leaves))) + mortons: $(VM)($(size(b.mortons))) order: $(VO)($(size(b.order))) """ ) end - +# Normal constructor which builds BVH function BVH( bounding_volumes::AbstractVector{L}, node_type::Type{N}=L, @@ -154,16 +171,7 @@ function BVH( tree = ImplicitTree{Int}(numbv) # Compute level up to which tree should be built - if built_level isa Integer - @assert 1 <= built_level <= tree.levels - built_ilevel = Int(built_level) - elseif built_level isa AbstractFloat - @assert 0 <= built_level <= 1 - built_ilevel = round(Int, tree.levels + (1 - tree.levels) * built_level) - else - throw(TypeError(:BVH, "built_level (the level to build BVH up to)", - Union{Integer, AbstractFloat}, typeof(built_level))) - end + built_ilevel = compute_build_level(tree, built_level) # Compute morton codes for the bounding volumes mortons = similar(bounding_volumes, morton_type) @@ -176,15 +184,83 @@ function BVH( # Pre-allocate vector of bounding volumes for the real nodes above the bottom level bvh_nodes = similar(bounding_volumes, N, tree.real_nodes - tree.real_leaves) - # Aggregate bounding volumes up to root + # Aggregate bounding volumes up to built_ilevel + if tree.real_nodes >= 2 + aggregate_oibvh!(bvh_nodes, bounding_volumes, tree, order, built_ilevel, num_threads) + end + + BVH(built_ilevel, tree, bvh_nodes, bounding_volumes, mortons, order) +end + + +# Copy constructor reusing previous memory; previous bounding volumes are moved to new_positions. +function BVH( + prev::BVH, + new_positions::AbstractMatrix, + built_level=1; + num_threads=Threads.nthreads(), +) + + # Ensure correctness + @assert size(new_positions, 1) == 3 "new_positions need 3D coordinates on first axis" + @assert size(new_positions, 2) == length(prev.leaves) "Different number of new_positions" + @assert firstindex(new_positions, 1) == 1 "BVH vector types must be 1-indexed" + @assert firstindex(new_positions, 2) == 1 "BVH vector types must be 1-indexed" + + # Extract previous BVH fields + tree = prev.tree + bvh_nodes = prev.nodes + bounding_volumes = prev.leaves + mortons = prev.mortons + order = prev.order + + # Compute level up to which tree should be built + built_ilevel = compute_build_level(tree, built_level) + + # Move the centres of previous bounding volumes to the coordinates of new_positions; ensure we + # use the same type as in prev + for i in axes(new_positions, 2) + bv = bounding_volumes[i] + c = center(bv) + T = eltype(c) + dx = (T(new_positions[1, i]) - c[1], + T(new_positions[2, i]) - c[2], + T(new_positions[3, i]) - c[3]) + bounding_volumes[i] = translate(bv, dx) + end + + # Recompute Morton codes + @inbounds morton_encode!(mortons, bounding_volumes, num_threads=num_threads) + + # Compute indices that sort codes along the Z-curve - closer objects have closer Morton codes + sortperm!(order, mortons) + + # Aggregate bounding volumes up to built_ilevel if tree.real_nodes >= 2 aggregate_oibvh!(bvh_nodes, bounding_volumes, tree, order, built_ilevel, num_threads) end - BVH(built_ilevel, tree, bvh_nodes, bounding_volumes, order) + BVH(built_ilevel, tree, bvh_nodes, bounding_volumes, mortons, order) end +# Compute level up to which tree should be built +function compute_build_level(tree, built_level) + + if built_level isa Integer + @assert 1 <= built_level <= tree.levels + built_ilevel = Int(built_level) + elseif built_level isa AbstractFloat + @assert 0 <= built_level <= 1 + built_ilevel = round(Int, tree.levels + (1 - tree.levels) * built_level) + else + throw(TypeError(:BVH, "built_level (the level to build BVH up to)", + Union{Integer, AbstractFloat}, typeof(built_level))) + end + + built_ilevel +end + # Build ImplicitBVH nodes above the leaf-level from the bottom up, inplace function aggregate_oibvh!(bvh_nodes, bvh_leaves, tree, order, built_level, num_threads) diff --git a/test/runtests.jl b/test/runtests.jl index 62fc01e..8a9f5f3 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -158,6 +158,13 @@ end c = a + a @test c.x ≈ a.x @test c.r ≈ a.r + + # Translating + a = BSphere((0., 0., 0.), 0.5) + dx = (1., 1., 1.) + b = ImplicitBVH.translate(a, dx) + @test b.x ≈ dx + @test b.r == a.r end @@ -221,6 +228,13 @@ end c = a + a @test c.lo ≈ a.lo @test c.up ≈ a.up + + # Translating + a = BBox((0., 0., 0.), (1., 1., 1.)) + dx = (1., 1., 1.) + b = ImplicitBVH.translate(a, dx) + @test b.lo ≈ dx + @test b.up ≈ a.up .+ dx end @@ -341,6 +355,8 @@ end @testset "bvh_single_bsphere_small_ordered" begin + using ImplicitBVH: center + # Simple, ordered bounding spheres traversal test bvs = [ BSphere([0., 0, 0], 0.5), @@ -389,7 +405,6 @@ end @test length(bvh.nodes) == 6 # Level 3 - center = ImplicitBVH.center @test center(bvh.nodes[4]) ≈ center(leaf(bvs[1], bvs[2])) # First two BVs are paired @test center(bvh.nodes[5]) ≈ center(leaf(bvs[3], bvs[4])) # Next two BVs are paired @test center(bvh.nodes[6]) ≈ center(bvs[5]) # Last BV has no pair @@ -408,6 +423,22 @@ end @test (4, 5) in traversal.contacts @test (1, 2) in traversal.contacts @test (2, 3) in traversal.contacts + + # Translate BVH + bvh = BVH(bvs) + new_positions = [ + 1. 0 0 + 1. 0 2 + 1. 0 4 + 1. 0 6 + 1. 0 8 + ]' + translated = BVH(bvh, new_positions) + @test center(translated.leaves[1]) ≈ (1, 0, 0) + @test center(translated.leaves[2]) ≈ (1, 0, 2) + @test center(translated.leaves[3]) ≈ (1, 0, 4) + @test center(translated.leaves[4]) ≈ (1, 0, 6) + @test center(translated.leaves[5]) ≈ (1, 0, 8) end