diff --git a/Cargo.lock b/Cargo.lock index 1c46c281a..7685135ed 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -198,6 +198,12 @@ dependencies = [ "num-traits", ] +[[package]] +name = "arbitrary-chunks" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2ad8689a486416c401ea15715a4694de30054248ec627edbf31f49cb64ee4086" + [[package]] name = "arrayref" version = "0.3.9" @@ -356,12 +362,12 @@ dependencies = [ "itertools 0.13.0", "libm", "nalgebra", + "obvhs", "parry2d", "parry2d-f64", "serde", - "slab", "smallvec", - "thiserror 2.0.17", + "thiserror 2.0.18", "thread_local", ] @@ -385,12 +391,13 @@ dependencies = [ "itertools 0.13.0", "libm", "nalgebra", + "obvhs", "parry3d", "parry3d-f64", + "rand", "serde", - "slab", "smallvec", - "thiserror 2.0.17", + "thiserror 2.0.18", "thread_local", ] @@ -469,7 +476,7 @@ dependencies = [ "ron", "serde", "smallvec", - "thiserror 2.0.17", + "thiserror 2.0.18", "thread_local", "tracing", "uuid", @@ -525,7 +532,7 @@ dependencies = [ "ctrlc", "downcast-rs 2.0.2", "log", - "thiserror 2.0.17", + "thiserror 2.0.18", "variadics_please", "wasm-bindgen", "web-sys", @@ -566,7 +573,7 @@ dependencies = [ "ron", "serde", "stackfuture", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", "ureq", "uuid", @@ -627,7 +634,7 @@ dependencies = [ "downcast-rs 2.0.2", "serde", "smallvec", - "thiserror 2.0.17", + "thiserror 2.0.18", "wgpu-types", ] @@ -643,7 +650,7 @@ dependencies = [ "derive_more", "encase", "serde", - "thiserror 2.0.17", + "thiserror 2.0.18", "wgpu-types", ] @@ -673,7 +680,7 @@ dependencies = [ "nonmax", "radsort", "smallvec", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", ] @@ -759,7 +766,7 @@ dependencies = [ "serde", "slotmap", "smallvec", - "thiserror 2.0.17", + "thiserror 2.0.18", "variadics_please", ] @@ -785,6 +792,36 @@ dependencies = [ "encase_derive_impl", ] +[[package]] +name = "bevy_feathers" +version = "0.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "07df7057ea9e2cf6d51fc6ccc3f15cf3694186995ddf394a59ec3ae5454985f1" +dependencies = [ + "accesskit", + "bevy_a11y", + "bevy_app", + "bevy_asset", + "bevy_camera", + "bevy_color", + "bevy_derive", + "bevy_ecs", + "bevy_input_focus", + "bevy_log", + "bevy_math", + "bevy_picking", + "bevy_platform", + "bevy_reflect", + "bevy_render", + "bevy_shader", + "bevy_text", + "bevy_ui", + "bevy_ui_render", + "bevy_ui_widgets", + "bevy_window", + "smol_str", +] + [[package]] name = "bevy_gilrs" version = "0.18.0" @@ -797,7 +834,7 @@ dependencies = [ "bevy_platform", "bevy_time", "gilrs", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", ] @@ -889,7 +926,7 @@ dependencies = [ "serde", "serde_json", "smallvec", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", ] @@ -930,7 +967,7 @@ dependencies = [ "rectangle-pack", "ruzstd", "serde", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", "wgpu-types", ] @@ -950,7 +987,7 @@ dependencies = [ "log", "serde", "smol_str", - "thiserror 2.0.17", + "thiserror 2.0.18", ] [[package]] @@ -967,7 +1004,7 @@ dependencies = [ "bevy_reflect", "bevy_window", "log", - "thiserror 2.0.17", + "thiserror 2.0.18", ] [[package]] @@ -990,6 +1027,7 @@ dependencies = [ "bevy_dev_tools", "bevy_diagnostic", "bevy_ecs", + "bevy_feathers", "bevy_gilrs", "bevy_gizmos", "bevy_gizmos_render", @@ -1019,6 +1057,7 @@ dependencies = [ "bevy_transform", "bevy_ui", "bevy_ui_render", + "bevy_ui_widgets", "bevy_utils", "bevy_window", "bevy_winit", @@ -1091,7 +1130,7 @@ dependencies = [ "rand", "rand_distr", "serde", - "thiserror 2.0.17", + "thiserror 2.0.18", "variadics_please", ] @@ -1116,7 +1155,7 @@ dependencies = [ "derive_more", "hexasphere", "serde", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", "wgpu-types", ] @@ -1177,7 +1216,7 @@ dependencies = [ "offset-allocator", "smallvec", "static_assertions", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", ] @@ -1251,7 +1290,7 @@ dependencies = [ "nonmax", "radsort", "smallvec", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", ] @@ -1284,7 +1323,7 @@ dependencies = [ "serde", "smallvec", "smol_str", - "thiserror 2.0.17", + "thiserror 2.0.18", "uuid", "variadics_please", "wgpu-types", @@ -1346,7 +1385,7 @@ dependencies = [ "offset-allocator", "send_wrapper", "smallvec", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", "variadics_please", "wasm-bindgen", @@ -1384,7 +1423,7 @@ dependencies = [ "derive_more", "ron", "serde", - "thiserror 2.0.17", + "thiserror 2.0.18", "uuid", ] @@ -1400,7 +1439,7 @@ dependencies = [ "naga", "naga_oil", "serde", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", "wgpu-types", ] @@ -1529,7 +1568,7 @@ dependencies = [ "serde", "smallvec", "sys-locale", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", "wgpu-types", ] @@ -1564,7 +1603,7 @@ dependencies = [ "bevy_utils", "derive_more", "serde", - "thiserror 2.0.17", + "thiserror 2.0.18", ] [[package]] @@ -1607,7 +1646,7 @@ dependencies = [ "serde", "smallvec", "taffy", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", "uuid", ] @@ -1643,6 +1682,26 @@ dependencies = [ "tracing", ] +[[package]] +name = "bevy_ui_widgets" +version = "0.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94a7db20f20f567e9078c5aaabfb53f602c2c59c11584c692951d94f675c21ea" +dependencies = [ + "accesskit", + "bevy_a11y", + "bevy_app", + "bevy_camera", + "bevy_ecs", + "bevy_input", + "bevy_input_focus", + "bevy_log", + "bevy_math", + "bevy_picking", + "bevy_reflect", + "bevy_ui", +] + [[package]] name = "bevy_utils" version = "0.18.0" @@ -1776,6 +1835,16 @@ version = "0.1.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0d8c1fef690941d3e7788d328517591fecc684c084084702d6ff1641e993699a" +[[package]] +name = "block-pseudorand" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2097358495d244a0643746f4d13eedba4608137008cf9dec54e53a3b700115a6" +dependencies = [ + "chiapos-chacha8", + "nanorand", +] + [[package]] name = "block2" version = "0.5.1" @@ -1885,9 +1954,9 @@ checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5" [[package]] name = "cc" -version = "1.2.53" +version = "1.2.54" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "755d2fce177175ffca841e9a06afdb2c4ab0f593d53b4dee48147dfaade85932" +checksum = "6354c81bbfd62d9cfa9cb3c773c2b7b2a3a482d569de977fd0e961f6e7c00583" dependencies = [ "find-msvc-tools", "jobserver", @@ -1922,6 +1991,15 @@ version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "613afe47fcd5fac7ccf1db93babcb082c5994d996f20b8b159f2ad1658eb5724" +[[package]] +name = "chiapos-chacha8" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "33f8be573a85f6c2bc1b8e43834c07e32f95e489b914bf856c0549c3c269cd0a" +dependencies = [ + "rayon", +] + [[package]] name = "ciborium" version = "0.2.2" @@ -2452,7 +2530,7 @@ checksum = "6e3e0ff2ee0b7aa97428308dd9e1e42369cb22f5fb8dc1c55546637443a60f1e" dependencies = [ "const_panic", "encase_derive", - "thiserror 2.0.17", + "thiserror 2.0.18", ] [[package]] @@ -2515,9 +2593,9 @@ dependencies = [ [[package]] name = "euclid" -version = "0.22.11" +version = "0.22.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ad9cdb4b747e485a12abb0e6566612956c7a1bafa3bdb8d682c5b6d403589e48" +checksum = "df61bf483e837f88d5c2291dcf55c67be7e676b3a51acc48db3a7b163b91ed63" dependencies = [ "num-traits", ] @@ -3393,9 +3471,9 @@ dependencies = [ [[package]] name = "libm" -version = "0.2.15" +version = "0.2.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de" +checksum = "b6d2cec3eae94f9f509c767b45932f1ada8350c4bdb85af2fcab4a3c14807981" [[package]] name = "libredox" @@ -3573,7 +3651,7 @@ dependencies = [ "pp-rs", "rustc-hash 1.1.0", "spirv", - "thiserror 2.0.17", + "thiserror 2.0.18", "unicode-ident", ] @@ -3589,7 +3667,7 @@ dependencies = [ "naga", "regex", "rustc-hash 1.1.0", - "thiserror 2.0.17", + "thiserror 2.0.18", "tracing", "unicode-ident", ] @@ -3638,6 +3716,12 @@ dependencies = [ "syn", ] +[[package]] +name = "nanorand" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "729eb334247daa1803e0a094d0a5c55711b85571179f5ec6e53eccfdf7008958" + [[package]] name = "ndk" version = "0.8.0" @@ -4084,6 +4168,19 @@ dependencies = [ "cc", ] +[[package]] +name = "obvhs" +version = "0.3.0" +source = "git+https://github.com/DGriffin91/obvhs?branch=insertion_removal#4735d721dbdc3b3af0e1379f4d5a825686a6e935" +dependencies = [ + "bytemuck", + "glam 0.30.10", + "half", + "log", + "rdst", + "static_assertions", +] + [[package]] name = "offset-allocator" version = "0.2.0" @@ -4117,9 +4214,9 @@ checksum = "d6790f58c7ff633d8771f42965289203411a5e5c68388703c06e14f24770b41e" [[package]] name = "openssl-probe" -version = "0.2.0" +version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9f50d9b3dabb09ecd771ad0aa242ca6894994c130308ca3d7684634df8037391" +checksum = "7c87def4c32ab89d880effc9e097653c8da5d6ef28e6b539d313baaacfbafcbe" [[package]] name = "orbclient" @@ -4205,7 +4302,7 @@ dependencies = [ "slab", "smallvec", "spade", - "thiserror 2.0.17", + "thiserror 2.0.18", ] [[package]] @@ -4235,7 +4332,7 @@ dependencies = [ "slab", "smallvec", "spade", - "thiserror 2.0.17", + "thiserror 2.0.18", ] [[package]] @@ -4268,7 +4365,7 @@ dependencies = [ "smallvec", "spade", "static_assertions", - "thiserror 2.0.17", + "thiserror 2.0.18", ] [[package]] @@ -4299,9 +4396,15 @@ dependencies = [ "slab", "smallvec", "spade", - "thiserror 2.0.17", + "thiserror 2.0.18", ] +[[package]] +name = "partition" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "947f833aaa585cf12b8ec7c0476c98784c49f33b861376ffc84ed92adebf2aba" + [[package]] name = "paste" version = "1.0.15" @@ -4503,9 +4606,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.105" +version = "1.0.106" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "535d180e0ecab6268a3e718bb9fd44db66bbbc256257165fc699dadf70d16fe7" +checksum = "8fd00f0bb2e90d81d1044c2b32617f68fcb9fa3bb7640c23e9c748e53fb30934" dependencies = [ "unicode-ident", ] @@ -4536,9 +4639,9 @@ dependencies = [ [[package]] name = "quote" -version = "1.0.43" +version = "1.0.44" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc74d9a594b72ae6656596548f56f667211f8a97b3d4c3d467150794690dc40a" +checksum = "21b2ebcf727b7760c461f091f9f0f539b77b8e87f2fd88131e7f1b433b3cece4" dependencies = [ "proc-macro2", ] @@ -4638,6 +4741,20 @@ dependencies = [ "crossbeam-utils", ] +[[package]] +name = "rdst" +version = "0.20.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6e7970b4e577b76a96d5e56b5f6662b66d1a4e1f5bb026ee118fc31b373c2752" +dependencies = [ + "arbitrary-chunks", + "block-pseudorand", + "criterion", + "partition", + "tikv-jemallocator", + "voracious_radix_sort", +] + [[package]] name = "read-fonts" version = "0.35.0" @@ -5329,11 +5446,11 @@ dependencies = [ [[package]] name = "thiserror" -version = "2.0.17" +version = "2.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f63587ca0f12b72a0600bcba1d40081f830876000bb46dd2337a3051618f4fc8" +checksum = "4288b5bcbc7920c07a1149a35cf9590a2aa808e0bc1eafaade0b80947865fbc4" dependencies = [ - "thiserror-impl 2.0.17", + "thiserror-impl 2.0.18", ] [[package]] @@ -5349,9 +5466,9 @@ dependencies = [ [[package]] name = "thiserror-impl" -version = "2.0.17" +version = "2.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3ff15c8ecd7de3849db632e14d18d2571fa09dfc5ed93479bc4485c7a517c913" +checksum = "ebc4ee7f67670e9b64d05fa4253e753e016c6c95ff35b89b7941d6b856dec1d5" dependencies = [ "proc-macro2", "quote", @@ -5367,6 +5484,26 @@ dependencies = [ "cfg-if", ] +[[package]] +name = "tikv-jemalloc-sys" +version = "0.5.4+5.3.0-patched" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9402443cb8fd499b6f327e40565234ff34dbda27460c5b47db0db77443dd85d1" +dependencies = [ + "cc", + "libc", +] + +[[package]] +name = "tikv-jemallocator" +version = "0.5.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "965fe0c26be5c56c94e38ba547249074803efd52adfb66de62107d95aab3eaca" +dependencies = [ + "libc", + "tikv-jemalloc-sys", +] + [[package]] name = "tiny-skia" version = "0.11.4" @@ -5649,9 +5786,9 @@ checksum = "09cc8ee72d2a9becf2f2febe0205bbed8fc6615b7cb429ad062dc7b7ddd036a9" [[package]] name = "uuid" -version = "1.19.0" +version = "1.20.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2e054861b4bd027cd373e18e8d8d8e6548085000e41290d95ce0c373a654b4a" +checksum = "ee48d38b119b0cd71fe4141b30f5ba9c7c5d9f4e7a3a8b4a674e4b6ef789976f" dependencies = [ "getrandom 0.3.4", "js-sys", @@ -5688,6 +5825,15 @@ version = "0.9.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" +[[package]] +name = "voracious_radix_sort" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "446e7ffcb6c27a71d05af7e51ef2ee5b71c48424b122a832f2439651e1914899" +dependencies = [ + "rayon", +] + [[package]] name = "walkdir" version = "2.5.0" @@ -5969,7 +6115,7 @@ dependencies = [ "raw-window-handle", "rustc-hash 1.1.0", "smallvec", - "thiserror 2.0.17", + "thiserror 2.0.18", "wgpu-core-deps-apple", "wgpu-core-deps-wasm", "wgpu-core-deps-windows-linux-android", @@ -6045,7 +6191,7 @@ dependencies = [ "raw-window-handle", "renderdoc-sys", "smallvec", - "thiserror 2.0.17", + "thiserror 2.0.18", "wasm-bindgen", "web-sys", "wgpu-types", @@ -6064,7 +6210,7 @@ dependencies = [ "js-sys", "log", "serde", - "thiserror 2.0.17", + "thiserror 2.0.18", "web-sys", ] @@ -6728,6 +6874,6 @@ checksum = "b97154e67e32c85465826e8bcc1c59429aaaf107c1e4a9e53c8d8ccd5eff88d0" [[package]] name = "zmij" -version = "1.0.14" +version = "1.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bd8f3f50b848df28f887acb68e41201b5aea6bc8a8dacc00fb40635ff9a72fea" +checksum = "dfcd145825aace48cff44a8844de64bf75feec3080e0aa5cdbde72961ae51a65" diff --git a/crates/avian2d/Cargo.toml b/crates/avian2d/Cargo.toml index 6022176ad..7f07f05ee 100644 --- a/crates/avian2d/Cargo.toml +++ b/crates/avian2d/Cargo.toml @@ -28,12 +28,7 @@ f64 = [] debug-plugin = ["bevy/bevy_gizmos", "bevy/bevy_render"] simd = ["parry2d?/simd-stable", "parry2d-f64?/simd-stable"] -parallel = [ - "dep:thread_local", - "bevy/multi_threaded", - "parry2d?/parallel", - "parry2d-f64?/parallel", -] +parallel = ["bevy/multi_threaded", "parry2d?/parallel", "parry2d-f64?/parallel"] enhanced-determinism = [ "dep:libm", "bevy_math/libm", @@ -95,15 +90,15 @@ approx = "0.5" parry2d = { version = "0.25", optional = true } parry2d-f64 = { version = "0.25", optional = true } nalgebra = { version = "0.34", features = ["convert-glam030"], optional = true } +obvhs = { git = "https://github.com/DGriffin91/obvhs", branch = "insertion_removal" } serde = { version = "1", features = ["derive"], optional = true } derive_more = "2" thiserror = "2" arrayvec = "0.7" smallvec = "1.15" -slab = ">=0.4.10" itertools = "0.13" bitflags = "2.5.0" -thread_local = { version = "1.1", optional = true } +thread_local = { version = "1.1" } disqualified = { version = "1.0" } [dev-dependencies] diff --git a/crates/avian3d/Cargo.toml b/crates/avian3d/Cargo.toml index ae234b547..e257ad177 100644 --- a/crates/avian3d/Cargo.toml +++ b/crates/avian3d/Cargo.toml @@ -29,12 +29,7 @@ f64 = [] debug-plugin = ["bevy/bevy_gizmos", "bevy/bevy_render"] simd = ["parry3d?/simd-stable", "parry3d-f64?/simd-stable"] -parallel = [ - "dep:thread_local", - "bevy/multi_threaded", - "parry3d?/parallel", - "parry3d-f64?/parallel", -] +parallel = ["bevy/multi_threaded", "parry3d?/parallel", "parry3d-f64?/parallel"] enhanced-determinism = [ "dep:libm", "bevy_math/libm", @@ -97,22 +92,28 @@ approx = "0.5" parry3d = { version = "0.25", optional = true } parry3d-f64 = { version = "0.25", optional = true } nalgebra = { version = "0.34", features = ["convert-glam030"], optional = true } +obvhs = { git = "https://github.com/DGriffin91/obvhs", branch = "insertion_removal" } serde = { version = "1", features = ["derive"], optional = true } derive_more = "2" thiserror = "2" smallvec = "1.15" -slab = ">=0.4.10" itertools = "0.13" bitflags = "2.5.0" -thread_local = { version = "1.1", optional = true } +thread_local = { version = "1.1" } disqualified = { version = "1.0" } [dev-dependencies] examples_common_3d = { path = "../examples_common_3d" } -bevy = { version = "0.18.0", features = ["3d", "ui", "https"] } +bevy = { version = "0.18.0", features = [ + "3d", + "ui", + "https", + "experimental_bevy_feathers", +] } bevy_heavy = { version = "0.4", features = ["approx"] } criterion = { version = "0.5", features = ["html_reports"] } bevy_mod_debugdump = { version = "0.15" } +rand = "0.9" [lints] workspace = true @@ -121,6 +122,10 @@ workspace = true # Enable features when building the docs on docs.rs features = ["diagnostic_ui"] +[[example]] +name = "bvh" +required-features = ["3d", "default-collider"] + [[example]] name = "dynamic_character_3d" required-features = ["3d", "default-collider", "bevy_scene"] diff --git a/crates/avian3d/examples/bvh.rs b/crates/avian3d/examples/bvh.rs new file mode 100644 index 000000000..67b96e13e --- /dev/null +++ b/crates/avian3d/examples/bvh.rs @@ -0,0 +1,474 @@ +//! Demonstrates Avian's BVH acceleration structures used for broad phase collision detection +//! and spatial queries. +//! +//! This example is primarily intended for performance testing and demonstration purposes, +//! not for practical use. +//! +//! The scene spawns a grid of colliders that move randomly each frame. +//! The size of the grid and the movement parameters can be adjusted via GUI controls. + +use avian3d::{math::*, prelude::*}; +use bevy::{ + color::palettes::tailwind::GRAY_400, + feathers::{ + FeathersPlugins, + constants::fonts::{BOLD, REGULAR}, + controls::{SliderProps, checkbox, radio, slider}, + dark_theme::create_dark_theme, + theme::UiTheme, + }, + prelude::*, + ui::Checked, + ui_widgets::{ + RadioButton, RadioGroup, SliderPrecision, SliderStep, ValueChange, observe, + slider_self_update, + }, +}; +use examples_common_3d::ExampleCommonPlugin; +use rand::Rng; + +fn main() { + let mut app = App::new(); + + // Add plugins relevant to the example. + app.add_plugins(( + DefaultPlugins.build().set(WindowPlugin { + primary_window: Some(Window { + title: "App".to_string(), + ..default() + }), + ..default() + }), + FeathersPlugins, + ExampleCommonPlugin, + PhysicsDebugPlugin, + )); + + // Add minimal physics plugins required for the example. + // TODO: Make these more minimal and ideally use more plugin groups. + app.add_plugins(( + PhysicsSchedulePlugin::default(), + ColliderHierarchyPlugin, + ColliderTransformPlugin::default(), + ColliderBackendPlugin::::default(), + ColliderTreePlugin::::default(), + BroadPhaseCorePlugin, + BvhBroadPhasePlugin::<()>::default(), + PhysicsTransformPlugin::default(), + // TODO: These are currently needed for collider tree updates, but they shouldn't be. + SolverBodyPlugin, + SolverSchedulePlugin, + )); + + // Configure gizmos and initialize example settings. + app.insert_gizmo_config( + PhysicsGizmos { + aabb_color: Some(GRAY_400.into()), + collider_tree_color: Some(Color::WHITE), + ..PhysicsGizmos::none() + }, + GizmoConfig { + line: GizmoLineConfig { + width: 0.5, + ..default() + }, + ..default() + }, + ) + .insert_resource(UiTheme(create_dark_theme())) + .init_resource::() + .insert_resource(Gravity::ZERO); + + // Add systems for setting up and running the example. + app.add_systems(Startup, (setup_scene, setup_ui)) + .add_systems(FixedUpdate, move_random); + + app.run(); +} + +const PARTICLE_RADIUS: f32 = 7.0; + +/// Settings for the BVH example. +#[derive(Resource)] +struct BvhExampleSettings { + x_count: usize, + y_count: usize, + move_fraction: f32, + delta_fraction: f32, +} + +impl Default for BvhExampleSettings { + fn default() -> Self { + Self { + x_count: 50, + y_count: 50, + move_fraction: 0.25, + delta_fraction: 0.1, + } + } +} + +/// Sets up the initial scene with a grid of colliders. +fn setup_scene(mut commands: Commands, settings: Res) { + let x_count = settings.x_count as isize; + let y_count = settings.y_count as isize; + + commands.spawn(( + Camera3d::default(), + Projection::Orthographic(OrthographicProjection { + scaling_mode: bevy::camera::ScalingMode::FixedVertical { + viewport_height: 3.0 * PARTICLE_RADIUS * (y_count as f32 * 1.2), + }, + ..OrthographicProjection::default_3d() + }), + Transform::from_xyz(0.0, 0.0, 30.0).looking_at(Vec3::ZERO, Vec3::Y), + )); + + for x in -x_count / 2..x_count / 2 { + for y in -y_count / 2..y_count / 2 { + commands.spawn(( + Transform::from_xyz( + (x as f32 + 0.5) * 3.0 * PARTICLE_RADIUS, + (y as f32 + 0.5) * 3.0 * PARTICLE_RADIUS, + 0.0, + ), + RigidBody::Dynamic, + SleepingDisabled, + Collider::sphere(PARTICLE_RADIUS.adjust_precision()), + CollisionLayers::new(LayerMask::DEFAULT, LayerMask::NONE), + )); + } + } +} + +/// Clears the scene of all rigid bodies and cameras. +#[expect(clippy::type_complexity)] +fn clear_scene(mut commands: Commands, query: Query, With)>>) { + for entity in query.iter() { + commands.entity(entity).despawn(); + } +} + +/// Moves a fraction of the colliders randomly each frame. +fn move_random(mut query: Query<&mut Position>, settings: Res) { + if settings.move_fraction <= 0.0 || settings.delta_fraction <= 0.0 { + return; + } + + let mut rng = rand::rng(); + for mut position in query.iter_mut() { + if rng.random::() < settings.move_fraction { + position.0 += Vec3::new( + rng.random_range( + -PARTICLE_RADIUS * settings.delta_fraction + ..PARTICLE_RADIUS * settings.delta_fraction, + ), + rng.random_range( + -PARTICLE_RADIUS * settings.delta_fraction + ..PARTICLE_RADIUS * settings.delta_fraction, + ), + 0.0, + ) + .adjust_precision(); + } + } +} + +// === UI Setup === + +#[derive(Component)] +struct OptimizationModeRadio(TreeOptimizationMode); + +#[derive(Component)] +struct GridSizeRadio(usize); + +// TODO: Change optimization settings at runtime. +fn setup_ui( + mut commands: Commands, + settings: Res, + asset_server: Res, +) { + let regular: Handle = asset_server.load(REGULAR); + let bold: Handle = asset_server.load(BOLD); + + commands.spawn(( + Name::new("Example Settings UI"), + Node { + position_type: PositionType::Absolute, + bottom: Val::Px(5.0), + right: Val::Px(5.0), + width: Val::Px(270.0), + padding: UiRect::all(Val::Px(10.0)), + border_radius: BorderRadius::all(Val::Px(5.0)), + flex_direction: FlexDirection::Column, + row_gap: Val::Px(15.0), + ..default() + }, + BackgroundColor(Color::srgba(0.0, 0.0, 0.0, 0.8)), + children![ + ( + Node { + flex_direction: FlexDirection::Column, + row_gap: Val::Px(5.0), + ..default() + }, + children![ + ( + Text::new("Optimization Mode"), + TextFont::from_font_size(14.0).with_font(bold.clone()) + ), + ( + Node { + display: Display::Flex, + flex_direction: FlexDirection::Column, + row_gap: px(5), + ..default() + }, + RadioGroup, + observe( + |value_change: On>, + radio_buttons: Query< + (Entity, &OptimizationModeRadio), + With, + >, + mut settings: ResMut, + mut commands: Commands| { + for (entity, optimization_mode) in radio_buttons.iter() { + if entity == value_change.value { + commands.entity(entity).insert(Checked); + if optimization_mode.0 == settings.optimization_mode { + continue; + } + settings.optimization_mode = optimization_mode.0; + commands.run_system_cached(clear_scene); + commands.run_system_cached(setup_scene); + } else { + commands.entity(entity).remove::(); + } + } + } + ), + children![ + radio( + OptimizationModeRadio(TreeOptimizationMode::Reinsert), + Spawn(( + Text::new("Reinsert"), + TextFont::from_font_size(13.0).with_font(regular.clone()) + )) + ), + radio( + OptimizationModeRadio(TreeOptimizationMode::PartialRebuild), + Spawn(( + Text::new("Partial Rebuild"), + TextFont::from_font_size(13.0).with_font(regular.clone()) + )) + ), + radio( + OptimizationModeRadio(TreeOptimizationMode::FullRebuild), + Spawn(( + Text::new("Full Rebuild"), + TextFont::from_font_size(13.0).with_font(regular.clone()) + )) + ), + radio( + ( + Checked, + OptimizationModeRadio(TreeOptimizationMode::default()) + ), + Spawn(( + Text::new("Adaptive"), + TextFont::from_font_size(13.0).with_font(regular.clone()) + )) + ), + ] + ), + ], + ), + ( + Node { + flex_direction: FlexDirection::Column, + row_gap: Val::Px(5.0), + ..default() + }, + children![ + ( + Text::new("Grid Size"), + TextFont::from_font_size(14.0).with_font(bold.clone()) + ), + ( + Node { + display: Display::Flex, + flex_direction: FlexDirection::Column, + row_gap: px(5), + ..default() + }, + RadioGroup, + observe( + |value_change: On>, + radio_buttons: Query<(Entity, &GridSizeRadio), With>, + mut settings: ResMut, + mut commands: Commands| { + for (entity, grid_size) in radio_buttons.iter() { + if entity == value_change.value { + commands.entity(entity).insert(Checked); + if grid_size.0 == settings.x_count { + continue; + } + settings.x_count = grid_size.0; + settings.y_count = grid_size.0; + commands.run_system_cached(clear_scene); + commands.run_system_cached(setup_scene); + } else { + commands.entity(entity).remove::(); + } + } + } + ), + children![ + radio( + GridSizeRadio(10), + Spawn(( + Text::new("10x10"), + TextFont::from_font_size(13.0).with_font(regular.clone()) + )) + ), + radio( + (Checked, GridSizeRadio(50)), + Spawn(( + Text::new("50x50"), + TextFont::from_font_size(13.0).with_font(regular.clone()) + )) + ), + radio( + GridSizeRadio(100), + Spawn(( + Text::new("100x100"), + TextFont::from_font_size(13.0).with_font(regular.clone()) + )) + ), + ] + ), + ], + ), + ( + Node { + flex_direction: FlexDirection::Column, + row_gap: Val::Px(5.0), + ..default() + }, + children![ + ( + Text::new("Move Fraction"), + TextFont::from_font_size(14.0).with_font(bold.clone()) + ), + ( + slider( + SliderProps { + min: 0.0, + max: 1.0, + value: settings.move_fraction, + }, + (SliderStep(0.05), SliderPrecision(2)), + ), + observe(slider_self_update), + observe( + |change: On>, + mut settings: ResMut| { + settings.move_fraction = change.value; + }, + ), + ) + ], + ), + ( + Node { + flex_direction: FlexDirection::Column, + row_gap: Val::Px(5.0), + ..default() + }, + children![ + ( + Text::new("Delta Fraction"), + TextFont::from_font_size(14.0).with_font(bold.clone()) + ), + ( + slider( + SliderProps { + min: 0.0, + max: 1.0, + value: settings.delta_fraction, + }, + (SliderStep(0.05), SliderPrecision(2)), + ), + observe(slider_self_update), + observe( + |change: On>, + mut settings: ResMut| { + settings.delta_fraction = change.value; + }, + ), + ) + ], + ), + ( + Node { + flex_direction: FlexDirection::Column, + row_gap: Val::Px(5.0), + ..default() + }, + children![ + ( + Text::new("BVH Debug Rendering"), + TextFont::from_font_size(14.0).with_font(bold) + ), + ( + checkbox( + Checked, + Spawn(( + Text::new("Draw Internal Nodes"), + TextFont::from_font_size(13.0).with_font(regular.clone()) + )) + ), + observe( + |change: On>, + mut gizmo_store: ResMut, + mut commands: Commands| { + let gizmo_config = gizmo_store.config_mut::().1; + if change.value { + gizmo_config.collider_tree_color = Some(Color::WHITE); + commands.entity(change.source).insert(Checked); + } else { + gizmo_config.collider_tree_color = None; + commands.entity(change.source).remove::(); + } + }, + ) + ), + ( + checkbox( + Checked, + Spawn(( + Text::new("Draw Leaf Nodes"), + TextFont::from_font_size(13.0).with_font(regular) + )) + ), + observe( + |change: On>, + mut gizmo_store: ResMut, + mut commands: Commands| { + let gizmo_config = gizmo_store.config_mut::().1; + if change.value { + gizmo_config.aabb_color = Some(GRAY_400.into()); + commands.entity(change.source).insert(Checked); + } else { + gizmo_config.aabb_color = None; + commands.entity(change.source).remove::(); + } + }, + ) + ) + ], + ), + ], + )); +} diff --git a/crates/avian3d/examples/custom_broad_phase.rs b/crates/avian3d/examples/custom_broad_phase.rs index 9c0541b56..cece0a001 100644 --- a/crates/avian3d/examples/custom_broad_phase.rs +++ b/crates/avian3d/examples/custom_broad_phase.rs @@ -11,7 +11,7 @@ fn main() { app.add_plugins( PhysicsPlugins::default() .build() - .disable::() + .disable::() .add(BruteForceBroadPhasePlugin), ); @@ -67,7 +67,7 @@ impl Plugin for BruteForceBroadPhasePlugin { // Add the broad phase system into the broad phase set. app.add_systems( PhysicsSchedule, - collect_collision_pairs.in_set(PhysicsStepSystems::BroadPhase), + collect_collision_pairs.in_set(BroadPhaseSystems::CollectCollisions), ); } } @@ -110,13 +110,10 @@ fn collect_collision_pairs( continue; } - // Create a contact pair as non-touching by adding an edge between the entities in the contact graph. + // Create a contact in the contact graph. let mut contact_edge = ContactEdge::new(collider1, collider2); contact_edge.body1 = Some(collider_of1.body); contact_edge.body2 = Some(collider_of2.body); - contact_graph.add_edge_with(contact_edge, |contact_pair| { - contact_pair.body1 = Some(collider_of1.body); - contact_pair.body2 = Some(collider_of2.body); - }); + contact_graph.add_edge(contact_edge); } } diff --git a/migration-guides/0.5-to-main.md b/migration-guides/0.5-to-main.md index 0965c96a7..afb0b13c6 100644 --- a/migration-guides/0.5-to-main.md +++ b/migration-guides/0.5-to-main.md @@ -6,6 +6,27 @@ since the latest release. These guides are evolving and may not be polished yet. See [migration-guides/README.md](./README.md) and existing entries for information about Avian's migration guide process and what to put here. +## Broad Phase Rework + +PR [#927](https://github.com/avianphysics/avian/pull/927) overhauled broad phase collision detection +to use a Bounding Volume Hierarchy (BVH). + +`BroadPhasePlugin` has been replaced by two plugins: + +- `BroadPhaseCorePlugin`: Sets up resources, system sets, and diagnostics required for broad phase collision detection. +- `BvhBroadPhasePlugin`: Implements a broad phase using a BVH to efficiently find overlapping AABBs. + +The latter can be swapped out for a custom broad phase implementation if desired. +See the documentation of the `broad_phase plugin for more information. + +The `BroadPhaseSystems::UpdateStructures` system set has also been removed. +The BVH acceleration structures are updated by the `ColliderTreePlugin`, +in `ColliderTreeSystems::UpdateAabbs`. + +Contact pair creation order can be different from before due to the new broad phase algorithm, +resulting in slightly changed behavior (but you should not rely on a specific contact order anyway). +The order should still be deterministic across runs, given the same inputs. + ## `ReadRigidBodyForces` and `WriteRigidBodyForces` PR [#908](https://github.com/avianphysics/avian/pull/908) introduced two new traits: `ReadRigidBodyForces` and `WriteRigidBodyForces`, and `RigidyBodyForces` is now defined as: diff --git a/src/collider_tree/diagnostics.rs b/src/collider_tree/diagnostics.rs new file mode 100644 index 000000000..35f9d3c62 --- /dev/null +++ b/src/collider_tree/diagnostics.rs @@ -0,0 +1,31 @@ +use bevy::{ + diagnostic::DiagnosticPath, + prelude::{ReflectResource, Resource}, + reflect::Reflect, +}; +use core::time::Duration; + +use crate::diagnostics::{PhysicsDiagnostics, impl_diagnostic_paths}; + +/// Diagnostics for [collider trees](crate::collider_tree). +#[derive(Resource, Debug, Default, Reflect)] +#[reflect(Resource, Debug)] +pub struct ColliderTreeDiagnostics { + /// Time spent optimizing [collider trees](crate::collider_tree). + pub optimize: Duration, + /// Time spent updating AABBs and BVH nodes. + pub update: Duration, +} + +impl PhysicsDiagnostics for ColliderTreeDiagnostics { + fn timer_paths(&self) -> Vec<(&'static DiagnosticPath, Duration)> { + vec![(Self::OPTIMIZE, self.optimize), (Self::UPDATE, self.update)] + } +} + +impl_diagnostic_paths! { + impl ColliderTreeDiagnostics { + OPTIMIZE: "avian/collider_tree/optimize", + UPDATE: "avian/collider_tree/update", + } +} diff --git a/src/collider_tree/mod.rs b/src/collider_tree/mod.rs new file mode 100644 index 000000000..c0c91a19f --- /dev/null +++ b/src/collider_tree/mod.rs @@ -0,0 +1,165 @@ +//! Tree acceleration structures for spatial queries on [colliders](crate::collision::collider::Collider). +//! +//! To speed up [broad phase](crate::collision::broad_phase) collision detection and [spatial queries](crate::spatial_query), +//! Avian maintains a [`ColliderTree`] structure for all colliders in the physics world. This is implemented as +//! a [Bounding Volume Hierarchy (BVH)][BVH], which accelerates querying for [AABB](crate::collision::collider::ColliderAabb) +//! overlaps, ray intersections, and more. +//! +//! Colliders of dynamic, kinematic, and static bodies are all stored in a separate [`ColliderTree`] +//! to allow efficiently querying for specific subsets of colliders and to optimize tree updates based on body type. +//! Trees for dynamic and kinematic bodies are rebuilt every physics step, while the static tree is incrementally updated +//! when static colliders are added, removed, or modified. The trees are stored in the [`ColliderTrees`] resource. +//! +//! [BVH]: https://en.wikipedia.org/wiki/Bounding_volume_hierarchy +//! +//! # Usage +//! +//! The collider trees are fairly low-level, and not intended to be used directly. +//! For spatial queries, consider using the higher-level [`SpatialQuery`] API instead, +//! and for broad phase collision detection, consider using the [`BvhBroadPhasePlugin`]. +//! +//! [`SpatialQuery`]: crate::spatial_query::SpatialQuery +//! [`BvhBroadPhasePlugin`]: crate::collision::broad_phase::BvhBroadPhasePlugin + +mod diagnostics; +mod optimization; +mod proxy_key; +mod tree; +mod update; + +pub use diagnostics::ColliderTreeDiagnostics; +pub use optimization::{ColliderTreeOptimization, TreeOptimizationMode}; +pub use proxy_key::{ColliderTreeProxyKey, ColliderTreeType, ProxyId}; +pub use tree::{ColliderTree, ColliderTreeProxy, ColliderTreeProxyFlags, ColliderTreeWorkspace}; +pub use update::MovedProxies; + +use optimization::ColliderTreeOptimizationPlugin; +use update::ColliderTreeUpdatePlugin; + +use core::marker::PhantomData; + +use crate::prelude::*; +use bevy::prelude::*; + +/// A plugin that manages [collider trees](crate::collider_tree) for a collider type `C`. +pub struct ColliderTreePlugin(PhantomData); + +impl Default for ColliderTreePlugin { + fn default() -> Self { + Self(PhantomData) + } +} + +impl Plugin for ColliderTreePlugin { + fn build(&self, app: &mut App) { + // Register required components. + let _ = app.try_register_required_components_with::(|| { + // Use a default proxy key. This will be overwritten when the proxy is actually created. + ColliderTreeProxyKey::PLACEHOLDER + }); + + // Add plugin for updating trees as colliders move. + app.add_plugins(ColliderTreeUpdatePlugin::::default()); + + // Add plugin for optimizing trees tp maintain good query performance. + if !app.is_plugin_added::() { + app.add_plugins(ColliderTreeOptimizationPlugin); + } + + // Initialize resources. + app.init_resource::() + .init_resource::(); + + // Configure system sets. + app.configure_sets( + PhysicsSchedule, + ColliderTreeSystems::UpdateAabbs + .in_set(PhysicsStepSystems::BroadPhase) + .after(BroadPhaseSystems::First) + .before(BroadPhaseSystems::CollectCollisions), + ); + app.configure_sets( + PhysicsSchedule, + ColliderTreeSystems::BeginOptimize.in_set(BroadPhaseSystems::Last), + ); + app.configure_sets( + PhysicsSchedule, + ColliderTreeSystems::EndOptimize.in_set(PhysicsStepSystems::Finalize), + ); + } + + fn finish(&self, app: &mut App) { + // Register timer diagnostics for collider trees. + app.register_physics_diagnostics::(); + } +} + +/// System sets for managing [`ColliderTrees`]. +#[derive(SystemSet, Clone, Copy, Debug, PartialEq, Eq, Hash)] +pub enum ColliderTreeSystems { + /// Updates the AABBs of colliders. + UpdateAabbs, + /// Begins optimizing acceleration structures to keep their query performance good. + /// + /// This runs concurrently with the simulation step as an async task. + BeginOptimize, + /// Completes the optimization of acceleration structures started in [`ColliderTreeSystems::BeginOptimize`]. + /// + /// This runs at the end of the simulation step. + EndOptimize, +} + +/// Trees for accelerating queries on a set of colliders. +/// +/// See the [`collider_tree`](crate::collider_tree) module for more information. +#[derive(Resource, Default)] +pub struct ColliderTrees { + /// A tree for the colliders of dynamic bodies. + pub dynamic_tree: ColliderTree, + /// A tree for the colliders of kinematic bodies. + pub kinematic_tree: ColliderTree, + /// A tree for the colliders of static bodies and colliders with no body. + pub static_tree: ColliderTree, + /// A tree for standalone colliders with no associated rigid body. + pub standalone_tree: ColliderTree, +} + +impl ColliderTrees { + /// Returns the tree for the given [`ColliderTreeType`]. + #[inline] + pub const fn tree_for_type(&self, tree_type: ColliderTreeType) -> &ColliderTree { + match tree_type { + ColliderTreeType::Dynamic => &self.dynamic_tree, + ColliderTreeType::Kinematic => &self.kinematic_tree, + ColliderTreeType::Static => &self.static_tree, + ColliderTreeType::Standalone => &self.standalone_tree, + } + } + + /// Returns a mutable reference to the tree for the given [`ColliderTreeType`]. + #[inline] + pub const fn tree_for_type_mut(&mut self, tree_type: ColliderTreeType) -> &mut ColliderTree { + match tree_type { + ColliderTreeType::Dynamic => &mut self.dynamic_tree, + ColliderTreeType::Kinematic => &mut self.kinematic_tree, + ColliderTreeType::Static => &mut self.static_tree, + ColliderTreeType::Standalone => &mut self.standalone_tree, + } + } + + /// Returns the proxy with the given [`ColliderTreeProxyKey`], if it exists. + #[inline] + pub fn get_proxy(&self, key: ColliderTreeProxyKey) -> Option<&ColliderTreeProxy> { + self.tree_for_type(key.tree_type()) + .proxies + .get(key.id().index()) + } + + /// Returns a mutable reference to the proxy with the given [`ColliderTreeProxyKey`], if it exists. + #[inline] + pub fn get_proxy_mut(&mut self, key: ColliderTreeProxyKey) -> Option<&mut ColliderTreeProxy> { + self.tree_for_type_mut(key.tree_type()) + .proxies + .get_mut(key.id().index()) + } +} diff --git a/src/collider_tree/optimization.rs b/src/collider_tree/optimization.rs new file mode 100644 index 000000000..346e9acd8 --- /dev/null +++ b/src/collider_tree/optimization.rs @@ -0,0 +1,266 @@ +use crate::{ + collider_tree::{ + ColliderTree, ColliderTreeDiagnostics, ColliderTreeSystems, ColliderTrees, MovedProxies, + }, + prelude::*, +}; +use bevy::{ + ecs::world::CommandQueue, + prelude::*, + tasks::{AsyncComputeTaskPool, Task, block_on}, +}; + +/// A plugin that optimizes the dynamic [`ColliderTree`] to maintain good query performance. +pub(super) struct ColliderTreeOptimizationPlugin; + +impl Plugin for ColliderTreeOptimizationPlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .init_resource::(); + + app.add_systems( + PhysicsSchedule, + optimize_trees.in_set(ColliderTreeSystems::BeginOptimize), + ); + + app.add_systems( + PhysicsSchedule, + block_on_optimize_trees.in_set(ColliderTreeSystems::EndOptimize), + ); + } +} + +/// Settings for optimizing the dynamic [`ColliderTree`]. +#[derive(Resource, Debug, Default, PartialEq, Reflect)] +pub struct ColliderTreeOptimization { + /// If `true`, tree optimization will be performed in-place with minimal allocations. + /// This has the downside that the tree will be unavailable for [spatial queries] + /// during the simulation step while the optimization is ongoing (ex: in [collision hooks]). + /// + /// Otherwise, parts of the the tree will be cloned for the optimization, + /// allowing spatial queries to use the old tree during the simulation step, + /// but incurring additional memory allocation overhead. + /// + /// For optimal performance, set this to `true` if your application + /// does not perform spatial queries during the simulation step. + /// + /// **Default**: `false` + /// + /// [spatial queries]: crate::spatial_query + /// [collision hooks]: crate::collision::hooks + pub optimize_in_place: bool, + + /// The optimization mode for the collider tree. + /// + /// **Default**: [`TreeOptimizationMode::Adaptive`] + pub optimization_mode: TreeOptimizationMode, +} + +/// The optimization mode for a dynamic [`ColliderTree`]. +#[derive(Clone, Copy, Debug, PartialEq, Reflect)] +pub enum TreeOptimizationMode { + /// The tree is optimized by reinserting [moved proxies](`MovedProxies`). + /// + /// This is the fastest method when only a small portion of proxies have moved, + /// but is less effective for large numbers of moved proxies. + Reinsert, + + /// The tree is optimized by performing a partial rebuild that only rebuilds + /// parts of the tree affected by [moved proxies](`MovedProxies`). + /// + /// This method is more effective than reinsertion when a moderate number of proxies + /// have moved. However, if a large portion of proxies have moved, a full rebuild + /// can be more effective and have less overhead. + PartialRebuild, + + /// The tree is optimized by performing a full rebuild. + /// + /// This method can produce the highest quality tree, and can have less overhead + /// than other methods when a large portion of proxies have moved. + /// This makes it suitable for highly dynamic scenes. + FullRebuild, + + /// The tree is optimized adaptively based on how many proxies have moved. + /// + /// - If the ratio of moved proxies to total proxies is below + /// `reinsert_threshold`, [`Reinsert`](TreeOptimizationMode::Reinsert) is used. + /// - If the ratio is between `reinsert_threshold` and `partial_rebuild_threshold`, + /// [`PartialRebuild`](TreeOptimizationMode::PartialRebuild) is used. + /// - Otherwise, [`FullRebuild`](TreeOptimizationMode::FullRebuild) is used. + /// + /// This is the default mode. + Adaptive { + /// The threshold ratio of moved proxies to total proxies + /// below which reinsertion is performed. + /// + /// **Default**: `0.15` + reinsert_threshold: f32, + + /// The threshold ratio of moved proxies to total proxies + /// below which a partial rebuild is performed. + /// + /// **Default**: `0.45` + partial_rebuild_threshold: f32, + }, +} + +impl Default for TreeOptimizationMode { + fn default() -> Self { + TreeOptimizationMode::Adaptive { + reinsert_threshold: 0.15, + partial_rebuild_threshold: 0.45, + } + } +} + +impl TreeOptimizationMode { + /// Resolves the optimization mode based on the ratio of moved proxies. + /// + /// `moved_ratio` is the ratio of moved proxies to total proxies in the tree. + #[inline] + pub fn resolve(&self, moved_ratio: f32) -> TreeOptimizationMode { + match self { + TreeOptimizationMode::Adaptive { + reinsert_threshold, + partial_rebuild_threshold, + } => { + if moved_ratio < *reinsert_threshold { + TreeOptimizationMode::Reinsert + } else if moved_ratio < *partial_rebuild_threshold { + TreeOptimizationMode::PartialRebuild + } else { + TreeOptimizationMode::FullRebuild + } + } + other => *other, + } + } +} + +/// A resource tracking the ongoing optimization task for the dynamic [`ColliderTree`]. +#[derive(Resource, Default)] +struct OptimizationTask { + /// The collider tree being optimized. + /// + /// This is taken from the dynamic tree before optimization begins, + /// and the optimized BVH is written back to the dynamic tree + /// when the optimization is complete. + tree: ColliderTree, + + /// The async task performing the rebuild. + task: Option>, +} + +/// Begins optimizing the dynamic [`ColliderTree`] to maintain good query performance. +/// +/// This spawns an async task that runs concurrently with the simulation step. +fn optimize_trees( + mut collider_trees: ResMut, + mut optimization: ResMut, + optimization_settings: Res, + moved_proxies: ResMut, + mut diagnostics: ResMut, +) { + let start = crate::utils::Instant::now(); + + let task_pool = AsyncComputeTaskPool::get(); + + // TODO: Do this for at least the kinematic tree as well. + // Use the dynamic tree's workspace for the optimization. + core::mem::swap( + &mut collider_trees.dynamic_tree.workspace, + &mut optimization.tree.workspace, + ); + + let moved_ratio = + moved_proxies.proxies().len() as f32 / collider_trees.dynamic_tree.proxies.len() as f32; + + let mut tree = core::mem::take(&mut optimization.tree); + + if optimization_settings.optimize_in_place { + core::mem::swap(&mut tree.bvh, &mut collider_trees.dynamic_tree.bvh); + } else { + // TODO: Can we avoid cloning the entire BVH? + tree.bvh.clone_from(&collider_trees.dynamic_tree.bvh); + } + + let task = match optimization_settings.optimization_mode.resolve(moved_ratio) { + TreeOptimizationMode::Reinsert => { + let moved_leaves = moved_proxies + .proxies() + .iter() + .map(|key| tree.bvh.primitives_to_nodes[key.id().index()]) + .collect::>(); + + spawn_optimization_task(task_pool, tree, move |tree| { + tree.optimize_candidates(&moved_leaves, 1); + }) + } + TreeOptimizationMode::PartialRebuild => { + let moved_leaves = moved_proxies + .proxies() + .iter() + .map(|key| tree.bvh.primitives_to_nodes[key.id().index()]) + .collect::>(); + + spawn_optimization_task(task_pool, tree, move |tree| { + tree.rebuild_partial(&moved_leaves); + }) + } + TreeOptimizationMode::FullRebuild => spawn_optimization_task(task_pool, tree, |tree| { + tree.rebuild_full(); + }), + + TreeOptimizationMode::Adaptive { .. } => unreachable!(), + }; + + optimization.task = Some(task); + + diagnostics.optimize += start.elapsed(); +} + +/// Spawns and returns an async task to optimize the given collider tree +/// using the provided optimization function. +fn spawn_optimization_task( + task_pool: &AsyncComputeTaskPool, + mut tree: ColliderTree, + optimize: impl FnOnce(&mut ColliderTree) + Send + 'static, +) -> Task { + task_pool.spawn(async move { + optimize(&mut tree); + + let mut command_queue = CommandQueue::default(); + command_queue.push(move |world: &mut World| { + let mut collider_trees = world + .get_resource_mut::() + .expect("ColliderTrees resource missing"); + collider_trees.dynamic_tree.bvh = tree.bvh; + }); + command_queue + }) +} + +/// Completes the optimization of the dynamic [`ColliderTree`] started in [`optimize_trees`]. +fn block_on_optimize_trees( + mut commands: Commands, + mut collider_trees: ResMut, + mut optimization: ResMut, + mut diagnostics: ResMut, +) { + let start = crate::utils::Instant::now(); + + if let Some(task) = &mut optimization.task { + let mut command_queue = block_on(task); + commands.append(&mut command_queue); + } + + optimization.task = None; + + // Restore the dynamic tree's workspace. + core::mem::swap( + &mut collider_trees.dynamic_tree.workspace, + &mut optimization.tree.workspace, + ); + + diagnostics.optimize += start.elapsed(); +} diff --git a/src/collider_tree/proxy_key.rs b/src/collider_tree/proxy_key.rs new file mode 100644 index 000000000..4f85ecafa --- /dev/null +++ b/src/collider_tree/proxy_key.rs @@ -0,0 +1,248 @@ +use core::hint::unreachable_unchecked; + +use bevy::{ecs::component::Component, reflect::Reflect}; + +use crate::prelude::RigidBody; + +/// A key for a proxy in a [`ColliderTree`], encoding both +/// the [`ProxyId`] and the [`ColliderTreeType`]. +/// +/// The tree type is stored in the lower 2 bits of the key, +/// leaving 30 bits for the [`ProxyId`]. +/// +/// [`ColliderTree`]: crate::collider_tree::ColliderTree +#[derive(Component, Clone, Copy, Debug, PartialEq, Eq, Hash, Reflect)] +pub struct ColliderTreeProxyKey(u32); + +impl ColliderTreeProxyKey { + /// A placeholder proxy key used before the proxy is actually created. + pub const PLACEHOLDER: Self = ColliderTreeProxyKey(u32::MAX); + + /// Creates a new [`ColliderTreeProxyKey`] from the given [`ProxyId`] and tree type. + #[inline] + pub const fn new(id: ProxyId, tree_type: ColliderTreeType) -> Self { + // Encode the tree type in the lower 2 bits. + ColliderTreeProxyKey((id.id() << 2) | (tree_type as u32)) + } + + /// Returns the [`ProxyId`] of the proxy. + #[inline] + pub const fn id(&self) -> ProxyId { + ProxyId::new(self.0 >> 2) + } + + /// Returns the [`ColliderTreeType`] of the proxy. + #[inline] + pub const fn tree_type(&self) -> ColliderTreeType { + match self.0 & 0b11 { + 0 => ColliderTreeType::Dynamic, + 1 => ColliderTreeType::Kinematic, + 2 => ColliderTreeType::Static, + 3 => ColliderTreeType::Standalone, + // Safety: Bitwise AND with 0b11 can only yield 0, 1, 2, or 3. + _ => unsafe { unreachable_unchecked() }, + } + } + + /// Returns the rigid body type associated with the proxy. + /// + /// If the proxy is a standalone collider with no body, returns `None`. + #[inline] + pub const fn body(&self) -> Option { + match self.0 & 0b11 { + 0 => Some(RigidBody::Dynamic), + 1 => Some(RigidBody::Kinematic), + 2 => Some(RigidBody::Static), + 3 => None, + // Safety: Bitwise AND with 0b11 can only yield 0, 1, 2, or 3. + _ => unsafe { unreachable_unchecked() }, + } + } + + /// Returns `true` if the proxy belongs to a dynamic body. + #[inline] + pub const fn is_dynamic(&self) -> bool { + if let Some(body) = self.body() { + body as u32 == RigidBody::Dynamic as u32 + } else { + false + } + } + + /// Returns `true` if the proxy belongs to a kinematic body. + #[inline] + pub const fn is_kinematic(&self) -> bool { + if let Some(body) = self.body() { + body as u32 == RigidBody::Kinematic as u32 + } else { + false + } + } + + /// Returns `true` if the proxy belongs to a static body. + #[inline] + pub const fn is_static(&self) -> bool { + if let Some(body) = self.body() { + body as u32 == RigidBody::Static as u32 + } else { + false + } + } + + /// Returns `true` if the proxy is a standalone collider with no body. + #[inline] + pub const fn is_standalone(&self) -> bool { + self.body().is_none() + } +} + +/// A stable identifier for a proxy in a [`ColliderTree`]. +/// +/// [`ColliderTree`]: crate::collider_tree::ColliderTree +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, Reflect)] +pub struct ProxyId(u32); + +impl ProxyId { + /// A placeholder proxy ID used before the proxy is actually created. + pub const PLACEHOLDER: Self = ProxyId(u32::MAX >> 2); + + /// Creates a new [`ProxyId`] from the given `u32` identifier. + /// + /// Only the lower 30 bits should be used for the ID. + /// + /// # Panics + /// + /// Panics if either of the upper 2 bits are set and `debug_assertions` are enabled. + #[inline] + pub const fn new(id: u32) -> Self { + debug_assert!(id < (1 << 30), "ProxyId can only use lower 30 bits"); + ProxyId(id) + } + + /// Returns the proxy ID as a `u32`. + #[inline] + pub const fn id(&self) -> u32 { + self.0 + } + + /// Returns the proxy ID as a `usize`. + #[inline] + pub const fn index(&self) -> usize { + self.0 as usize + } +} + +impl From for ProxyId { + #[inline] + fn from(id: u32) -> Self { + ProxyId::new(id) + } +} + +impl From for u32 { + #[inline] + fn from(proxy_id: ProxyId) -> Self { + proxy_id.id() + } +} + +impl PartialOrd for ProxyId { + #[inline] + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} + +impl Ord for ProxyId { + #[inline] + fn cmp(&self, other: &Self) -> core::cmp::Ordering { + self.0.cmp(&other.0) + } +} + +/// The type of a collider tree, corresponding to the rigid body type. +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, Reflect)] +pub enum ColliderTreeType { + /// A tree for dynamic bodies. + Dynamic = 0, + /// A tree for kinematic bodies. + Kinematic = 1, + /// A tree for static bodies. + Static = 2, + /// A tree for standalone colliders with no associated rigid body. + Standalone = 3, +} + +impl ColliderTreeType { + /// Creates a new [`ColliderTreeType`] from the given optional rigid body type. + /// + /// `None` corresponds to standalone colliders with no body. + #[inline] + pub const fn from_body(body: Option) -> Self { + match body { + Some(RigidBody::Dynamic) => ColliderTreeType::Dynamic, + Some(RigidBody::Kinematic) => ColliderTreeType::Kinematic, + Some(RigidBody::Static) => ColliderTreeType::Static, + None => ColliderTreeType::Standalone, + } + } + + /// Returns `true` if the tree type is for dynamic bodies. + #[inline] + pub const fn is_dynamic(&self) -> bool { + matches!(self, ColliderTreeType::Dynamic) + } + + /// Returns `true` if the tree type is for kinematic bodies. + #[inline] + pub const fn is_kinematic(&self) -> bool { + matches!(self, ColliderTreeType::Kinematic) + } + + /// Returns `true` if the tree type is for static bodies. + #[inline] + pub const fn is_static(&self) -> bool { + matches!(self, ColliderTreeType::Static) + } + + /// Returns `true` if the tree type is for standalone colliders with no body. + #[inline] + pub const fn is_standalone(&self) -> bool { + matches!(self, ColliderTreeType::Standalone) + } +} + +impl From> for ColliderTreeType { + #[inline] + fn from(body: Option) -> Self { + match body { + Some(RigidBody::Dynamic) => ColliderTreeType::Dynamic, + Some(RigidBody::Kinematic) => ColliderTreeType::Kinematic, + Some(RigidBody::Static) => ColliderTreeType::Static, + None => ColliderTreeType::Standalone, + } + } +} + +impl From for Option { + #[inline] + fn from(tree_type: ColliderTreeType) -> Self { + match tree_type { + ColliderTreeType::Dynamic => Some(RigidBody::Dynamic), + ColliderTreeType::Kinematic => Some(RigidBody::Kinematic), + ColliderTreeType::Static => Some(RigidBody::Static), + ColliderTreeType::Standalone => None, + } + } +} + +impl From for ColliderTreeType { + #[inline] + fn from(body: RigidBody) -> Self { + match body { + RigidBody::Dynamic => ColliderTreeType::Dynamic, + RigidBody::Kinematic => ColliderTreeType::Kinematic, + RigidBody::Static => ColliderTreeType::Static, + } + } +} diff --git a/src/collider_tree/tree.rs b/src/collider_tree/tree.rs new file mode 100644 index 000000000..0bfc906ed --- /dev/null +++ b/src/collider_tree/tree.rs @@ -0,0 +1,346 @@ +use bevy::{ + ecs::{entity::Entity, resource::Resource}, + reflect::prelude::*, +}; +use obvhs::{ + INVALID, + aabb::Aabb, + bvh2::{Bvh2, insertion_removal::SiblingInsertionCandidate, reinsertion::ReinsertionOptimizer}, + faststack::HeapStack, + ploc::{ + PlocBuilder, PlocSearchDistance, SortPrecision, partial_rebuild::compute_rebuild_path_flags, + }, +}; + +use crate::{ + collider_tree::ProxyId, + data_structures::stable_vec::StableVec, + prelude::{ActiveCollisionHooks, CollisionLayers}, +}; + +/// A [Bounding Volume Hierarchy (BVH)][BVH] for accelerating queries on a set of colliders. +/// +/// See the [`collider_tree`](crate::collider_tree) module for more information. +/// +/// [BVH]: https://en.wikipedia.org/wiki/Bounding_volume_hierarchy +#[derive(Clone, Default)] +pub struct ColliderTree { + /// The underlying BVH structure. + pub bvh: Bvh2, + /// The proxies stored in the tree. + pub proxies: StableVec, + /// A list of moved proxies since the last update. + /// + /// This is used during tree optimization to determine + /// which proxies need to be reinserted or rebuilt. + pub moved_proxies: Vec, + /// A workspace for reusing allocations across tree operations. + pub workspace: ColliderTreeWorkspace, +} + +/// A proxy representing a collider in the [`ColliderTree`]. +#[derive(Clone, Debug)] +pub struct ColliderTreeProxy { + /// The collider entity this proxy represents. + pub collider: Entity, + /// The body this collider is attached to. + pub body: Option, + /// The tight AABB of the collider. + pub aabb: Aabb, + /// The collision layers of the collider. + pub layers: CollisionLayers, + /// Flags for the proxy. + pub flags: ColliderTreeProxyFlags, +} + +/// Flags for a [`ColliderTreeProxy`]. +#[repr(transparent)] +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Reflect)] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))] +#[reflect(Debug, PartialEq)] +pub struct ColliderTreeProxyFlags(u32); + +bitflags::bitflags! { + impl ColliderTreeProxyFlags: u32 { + /// Set if the collider is a sensor. + const SENSOR = 1 << 0; + /// Set if the body this collider is attached to has [`RigidBodyDisabled`](crate::dynamics::rigid_body::RigidBodyDisabled). + const BODY_DISABLED = 1 << 1; + /// Set if the custom filtering hook is active for this collider. + const CUSTOM_FILTER = 1 << 2; + /// Set if the contact modification hook is active for this collider. + const MODIFY_CONTACTS = 1 << 3; + /// Set if contact events are enabled for this collider. + const CONTACT_EVENTS = 1 << 4; + } +} + +impl ColliderTreeProxyFlags { + /// Creates [`ColliderTreeProxyFlags`] from the given sensor status and active collision hooks. + #[inline] + pub fn new( + is_sensor: bool, + is_body_disabled: bool, + events_enabled: bool, + active_hooks: ActiveCollisionHooks, + ) -> Self { + let mut flags = ColliderTreeProxyFlags::empty(); + if is_sensor { + flags |= ColliderTreeProxyFlags::SENSOR; + } + if is_body_disabled { + flags |= ColliderTreeProxyFlags::BODY_DISABLED; + } + if active_hooks.contains(ActiveCollisionHooks::FILTER_PAIRS) { + flags |= ColliderTreeProxyFlags::CUSTOM_FILTER; + } + if active_hooks.contains(ActiveCollisionHooks::MODIFY_CONTACTS) { + flags |= ColliderTreeProxyFlags::MODIFY_CONTACTS; + } + if events_enabled { + flags |= ColliderTreeProxyFlags::CONTACT_EVENTS; + } + flags + } +} + +impl ColliderTreeProxy { + /// Returns `true` if the collider is a sensor. + #[inline] + pub fn is_sensor(&self) -> bool { + self.flags.contains(ColliderTreeProxyFlags::SENSOR) + } + + /// Returns `true` if the custom filtering hook is active. + #[inline] + pub fn has_custom_filter(&self) -> bool { + self.flags.contains(ColliderTreeProxyFlags::CUSTOM_FILTER) + } + + /// Returns `true` if the contact modification hook is active. + #[inline] + pub fn has_contact_modification(&self) -> bool { + self.flags.contains(ColliderTreeProxyFlags::MODIFY_CONTACTS) + } +} + +/// A workspace for performing operations on a [`ColliderTree`]. +/// +/// This stores temporary data structures and working memory used when modifying the tree. +/// It is recommended to reuse a single instance of this struct for all operations on a tree +/// to avoid unnecessary allocations. +#[derive(Resource)] +pub struct ColliderTreeWorkspace { + /// Builds the tree using PLOC (*Parallel, Locally Ordered Clustering*). + pub ploc_builder: PlocBuilder, + /// Restructures the BVH, optimizing node locations within the BVH hierarchy per SAH cost. + pub reinsertion_optimizer: ReinsertionOptimizer, + /// A stack for tracking insertion candidates during proxy insertions. + pub insertion_stack: HeapStack, + /// A temporary BVH used during partial rebuilds. + pub temp_bvh: Bvh2, + /// Temporary flagged nodes for partial rebuilds. + pub temp_flags: Vec, +} + +impl Clone for ColliderTreeWorkspace { + fn clone(&self) -> Self { + Self { + ploc_builder: self.ploc_builder.clone(), + reinsertion_optimizer: ReinsertionOptimizer::default(), + insertion_stack: self.insertion_stack.clone(), + temp_bvh: Bvh2::default(), + temp_flags: Vec::new(), + } + } +} + +impl Default for ColliderTreeWorkspace { + fn default() -> Self { + Self { + ploc_builder: PlocBuilder::default(), + reinsertion_optimizer: ReinsertionOptimizer::default(), + insertion_stack: HeapStack::new_with_capacity(2000), + temp_bvh: Bvh2::default(), + temp_flags: Vec::new(), + } + } +} + +impl ColliderTree { + /// Adds a proxy to the tree, returning its index. + #[inline] + pub fn add_proxy(&mut self, aabb: Aabb, proxy: ColliderTreeProxy) -> ProxyId { + let id = self.proxies.push(proxy) as u32; + self.bvh + .insert_primitive(aabb, id, &mut self.workspace.insertion_stack); + ProxyId::new(id) + } + + /// Removes a proxy from the tree. + /// + /// Returns `true` if the proxy was successfully removed, or `false` if the proxy ID was invalid. + #[inline] + pub fn remove_proxy(&mut self, proxy_id: ProxyId) -> Option { + if let Some(proxy) = self.proxies.try_remove(proxy_id.index()) { + self.bvh.remove_primitive(proxy_id.id()); + Some(proxy) + } else { + None + } + } + + /// Gets a proxy from the tree by its ID. + /// + /// Returns `None` if the proxy ID is invalid. + #[inline] + pub fn get_proxy(&self, proxy_id: ProxyId) -> Option<&ColliderTreeProxy> { + self.proxies.get(proxy_id.index()) + } + + /// Gets a mutable reference to a proxy from the tree by its ID. + /// + /// Returns `None` if the proxy ID is invalid. + #[inline] + pub fn get_proxy_mut(&mut self, proxy_id: ProxyId) -> Option<&mut ColliderTreeProxy> { + self.proxies.get_mut(proxy_id.index()) + } + + /// Gets a proxy from the tree by its ID without bounds checking. + /// + /// # Safety + /// + /// The caller must ensure that the `proxy_id` is valid. + #[inline] + pub unsafe fn get_proxy_unchecked(&self, proxy_id: ProxyId) -> &ColliderTreeProxy { + unsafe { self.proxies.get_unchecked(proxy_id.index()) } + } + + /// Gets a mutable reference to a proxy from the tree by its ID without bounds checking. + /// + /// # Safety + /// + /// The caller must ensure that the `proxy_id` is valid. + #[inline] + pub unsafe fn get_proxy_unchecked_mut(&mut self, proxy_id: ProxyId) -> &mut ColliderTreeProxy { + unsafe { self.proxies.get_unchecked_mut(proxy_id.index()) } + } + + /// Updates the AABB of a proxy in the tree. + /// + /// If the BVH should be refitted at the same time, consider using + /// [`resize_proxy_aabb`](Self::resize_proxy_aabb) instead. + /// + /// If resizing a large number of proxies, consider calling this method + /// for each proxy and then calling [`refit_all`](Self::refit_all) once at the end. + #[inline] + pub fn set_proxy_aabb(&mut self, proxy_id: ProxyId, aabb: Aabb) { + // Get the node index for the proxy. + let node_index = self.bvh.primitives_to_nodes[proxy_id.index()] as usize; + + // Update the proxy's AABB in the BVH. + self.bvh.nodes[node_index].set_aabb(aabb); + } + + /// Resizes the AABB of a proxy in the tree. + /// + /// This is equivalent to calling [`set_proxy_aabb`](Self::set_proxy_aabb) + /// and then refitting the BVH working up from the resized node. + /// + /// For resizing a large number of proxies, consider calling [`set_proxy_aabb`](Self::set_proxy_aabb) + /// for each proxy and then calling [`refit_all`](Self::refit_all) once at the end. + #[inline] + pub fn resize_proxy_aabb(&mut self, proxy_id: ProxyId, aabb: Aabb) { + let node_index = self.bvh.primitives_to_nodes[proxy_id.index()] as usize; + self.bvh.resize_node(node_index, aabb); + } + + /// Updates the AABB of a proxy and reinserts it at an optimal place in the tree. + #[inline] + pub fn reinsert_proxy(&mut self, proxy_id: ProxyId, aabb: Aabb) { + // Update the proxy's AABB. + self.proxies[proxy_id.index()].aabb = aabb; + + // Reinsert the node into the BVH. + let node_id = self.bvh.primitives_to_nodes[proxy_id.index()]; + self.bvh.resize_node(node_id as usize, aabb); + self.bvh.reinsert_node(node_id as usize); + } + + /// Refits the entire tree from the leaves up. + #[inline] + pub fn refit_all(&mut self) { + self.bvh.refit_all(); + } + + /// Fully rebuilds the tree from the given list of AABBs. + #[inline] + pub fn rebuild_full(&mut self) { + if self.bvh.nodes.is_empty() { + return; + } + + self.bvh.init_primitives_to_nodes_if_uninit(); + + let mut aabbs: Vec = Vec::with_capacity(self.bvh.primitives_to_nodes.len()); + let mut indices: Vec = Vec::with_capacity(self.bvh.primitives_to_nodes.len()); + + for (i, &node_index) in self.bvh.primitives_to_nodes.iter().enumerate() { + if node_index == INVALID { + continue; + } + aabbs.push(self.bvh.nodes[node_index as usize].aabb); + indices.push(i as u32); + } + + self.workspace.ploc_builder.build_with_bvh( + &mut self.bvh, + PlocSearchDistance::Minimum, + &aabbs, + indices, + SortPrecision::U64, + 0, + ); + } + + /// Rebuilds parts of the tree corresponding to the given list of leaf node indices. + #[inline] + pub fn rebuild_partial(&mut self, leaves: &[u32]) { + self.bvh.init_parents_if_uninit(); + + // TODO: We could maybe get flagged nodes while refitting the tree. + compute_rebuild_path_flags(&self.bvh, leaves, &mut self.workspace.temp_flags); + + self.workspace.ploc_builder.partial_rebuild( + &mut self.bvh, + |node_id| self.workspace.temp_flags[node_id], + PlocSearchDistance::Minimum, + SortPrecision::U64, + 0, + ); + } + + /// Restructures the tree using parallel reinsertion, optimizing node locations based on SAH cost. + /// + /// This can be used to improve query performance after the tree quality has degraded, + /// for example after many proxy insertions and removals. + #[inline] + pub fn optimize(&mut self, batch_size_ratio: f32) { + self.workspace + .reinsertion_optimizer + .run(&mut self.bvh, batch_size_ratio, None); + } + + /// Restructures the tree using parallel reinsertion, optimizing node locations based on SAH cost. + /// + /// Only the specified candidate proxies are considered for reinsertion. + #[inline] + pub fn optimize_candidates(&mut self, candidates: &[u32], iterations: u32) { + self.workspace.reinsertion_optimizer.run_with_candidates( + &mut self.bvh, + candidates, + iterations, + ); + } +} diff --git a/src/collider_tree/update.rs b/src/collider_tree/update.rs new file mode 100644 index 000000000..b26bf56bb --- /dev/null +++ b/src/collider_tree/update.rs @@ -0,0 +1,1019 @@ +use core::cell::RefCell; +use core::marker::PhantomData; + +use crate::{ + collider_tree::{ + ColliderTreeDiagnostics, ColliderTreeProxy, ColliderTreeProxyKey, ColliderTreeSystems, + ColliderTreeType, ColliderTrees, ProxyId, tree::ColliderTreeProxyFlags, + }, + collision::collider::EnlargedAabb, + data_structures::bit_vec::BitVec, + dynamics::solver::solver_body::SolverBody, + prelude::*, +}; +use bevy::{ + ecs::{ + change_detection::Tick, + entity_disabling::Disabled, + query::QueryFilter, + system::{StaticSystemParam, SystemChangeTick}, + }, + platform::collections::HashSet, + prelude::*, +}; +use obvhs::aabb::Aabb; +use thread_local::ThreadLocal; + +/// A plugin for updating [`ColliderTree`]s for a collider type `C`. +/// +/// [`ColliderTree`]: crate::collider_tree::ColliderTree +pub(super) struct ColliderTreeUpdatePlugin(PhantomData); + +impl Default for ColliderTreeUpdatePlugin { + fn default() -> Self { + Self(PhantomData) + } +} + +impl Plugin for ColliderTreeUpdatePlugin { + fn build(&self, app: &mut App) { + // Initialize resources. + app.init_resource::() + .init_resource::() + .init_resource::(); + + // Add systems for updating collider AABBs before physics step. + // This accounts for manually moved colliders. + app.add_systems( + PhysicsSchedule, + ( + update_dynamic_kinematic_aabbs::, + update_static_aabbs::, + update_standalone_aabbs::, + ) + .chain() + .in_set(ColliderTreeSystems::UpdateAabbs) + // Allowing ambiguities is required so that it's possible + // to have multiple collision backends at the same time. + .ambiguous_with_all(), + ); + + // Clear moved proxies and update dynamic and kinematic collider AABBs. + app.add_systems( + PhysicsSchedule, + (clear_moved_proxies, update_dynamic_kinematic_aabbs::) + .chain() + .after(PhysicsStepSystems::Finalize) + .before(PhysicsStepSystems::Last), + ); + + // Initialize `ColliderAabb` for colliders. + app.add_observer( + |trigger: On, + mut query: Query<( + &C, + &Position, + &Rotation, + Option<&CollisionMargin>, + &mut ColliderAabb, + &mut EnlargedAabb, + )>, + narrow_phase_config: Res, + length_unit: Res, + collider_context: StaticSystemParam| { + let contact_tolerance = length_unit.0 * narrow_phase_config.contact_tolerance; + let aabb_context = AabbContext::new(trigger.entity, &*collider_context); + + if let Ok((collider, pos, rot, collision_margin, mut aabb, mut enlarged_aabb)) = + query.get_mut(trigger.entity) + { + // TODO: Should we instead do this in `add_to_tree_on`? + let collision_margin = collision_margin.map_or(0.0, |m| m.0); + *aabb = collider + .aabb_with_context(pos.0, *rot, aabb_context) + .grow(Vector::splat(contact_tolerance + collision_margin)); + enlarged_aabb.update(&aabb, 0.0); + } + }, + ); + + // Aside from AABB updates, we need to handle the following cases: + // + // 1. On insert `C` or `ColliderOf`, add to new tree if not already present. Remove from old tree if present. + // 2. On remove `C`, remove from tree. + // 3. On remove `ColliderOf`, move to standalone tree if `C` still exists. + // 4. On re-enable `C`, add to tree. + // 5. On disable `C`, remove from tree. + // 6. On replace `RigidBody`, move attached colliders to new tree. + // 7. On add `Sensor`, set sensor proxy flag. + // 8. On remove `Sensor`, unset sensor proxy flag. + // 9. On replace `CollisionLayers`, update proxy layers. + // 10. On replace `ActiveCollisionHooks`, set proxy flag. + // 11. On replace `RigidBodyDisabled`, set/unset proxy flag. + + // Case 1 + app.add_observer(add_to_tree_on::>); + + // Case 2 + // Note: We also include disabled entities here for the edge case where + // we despawn a disabled collider, which causes Case 4 to trigger first. + // Ideally Case 4 would not trigger for despawned entities. + // TODO: Clean up the edge case described above. + app.add_observer(remove_from_tree_on::>); + + // Case 3 + app.add_observer( + |trigger: On, + mut collider_query: Query< + ( + &ColliderTreeProxyKey, + &ColliderAabb, + &EnlargedAabb, + Option<&CollisionLayers>, + Has, + Has, + Option<&ActiveCollisionHooks>, + ), + (With, Without), + >, + mut trees: ResMut, + mut moved_proxies: ResMut| { + let entity = trigger.entity; + + let Ok(( + proxy_key, + collider_aabb, + enlarged_aabb, + layers, + is_sensor, + has_contact_events, + active_hooks, + )) = collider_query.get_mut(entity) + else { + return; + }; + + // Remove the proxy from its current tree. + let tree = trees.tree_for_type_mut(proxy_key.tree_type()); + if tree.remove_proxy(proxy_key.id()).is_none() { + return; + } + moved_proxies.remove(proxy_key); + + // If the collider still exists, move it to the standalone tree. + let aabb = Aabb::from(*collider_aabb); + let enlarged_aabb = Aabb::from(enlarged_aabb.get()); + + let proxy = ColliderTreeProxy { + collider: entity, + body: None, + layers: layers.copied().unwrap_or_default(), + aabb, + flags: ColliderTreeProxyFlags::new( + is_sensor, + false, + has_contact_events, + active_hooks.copied().unwrap_or_default(), + ), + }; + + let standalone_tree = &mut trees.standalone_tree; + let proxy_id = standalone_tree.add_proxy(enlarged_aabb, proxy); + let new_proxy_key = + ColliderTreeProxyKey::new(proxy_id, ColliderTreeType::Standalone); + + // Mark the proxy as moved. + moved_proxies.insert(new_proxy_key); + }, + ); + + // Cases 4 + // Note: We use `Replace` here to run before Case 2. + app.add_observer( + add_to_tree_on::, Allow)>, + ); + app.add_observer(add_to_tree_on::); + + // Case 5 + app.add_observer( + remove_from_tree_on::, Allow)>, + ); + app.add_observer(remove_from_tree_on::); + + // Case 6 + app.add_observer( + |trigger: On, + body_query: Query<(&RigidBody, &RigidBodyColliders, Has)>, + mut collider_query: Query< + ( + &ColliderAabb, + &EnlargedAabb, + &mut ColliderTreeProxyKey, + Option<&CollisionLayers>, + Has, + Has, + Option<&ActiveCollisionHooks>, + ), + Without, + >, + mut trees: ResMut, + mut moved_proxies: ResMut| { + let entity = trigger.entity; + + let Ok((new_rb, body_colliders, is_body_disabled)) = body_query.get(entity) else { + return; + }; + + for collider_entity in body_colliders.iter() { + let Ok(( + collider_aabb, + enlarged_aabb, + mut proxy_key, + layers, + is_sensor, + has_contact_events, + active_hooks, + )) = collider_query.get_mut(collider_entity) + else { + continue; + }; + + let new_tree_type = ColliderTreeType::from_body(Some(*new_rb)); + + if new_tree_type == proxy_key.tree_type() { + // No tree change. + break; + } + + // Remove the old proxy from its current tree. + let old_tree = trees.tree_for_type_mut(proxy_key.tree_type()); + old_tree.remove_proxy(proxy_key.id()); + moved_proxies.remove(&proxy_key); + + // Insert the proxy into the new tree. + let aabb = Aabb::from(*collider_aabb); + let enlarged_aabb = Aabb::from(enlarged_aabb.get()); + + let proxy = ColliderTreeProxy { + collider: collider_entity, + body: Some(entity), + layers: layers.copied().unwrap_or_default(), + aabb, + flags: ColliderTreeProxyFlags::new( + is_sensor, + is_body_disabled, + has_contact_events, + active_hooks.copied().unwrap_or_default(), + ), + }; + + let new_tree = trees.tree_for_type_mut(new_tree_type); + let proxy_id = new_tree.add_proxy(enlarged_aabb, proxy); + let new_proxy_key = ColliderTreeProxyKey::new(proxy_id, new_tree_type); + + // Store the new proxy key. + *proxy_key = new_proxy_key; + + // Mark the proxy as moved. + moved_proxies.insert(new_proxy_key); + } + }, + ); + + // Case 7 + app.add_observer( + |trigger: On, + mut collider_query: Query<&ColliderTreeProxyKey, Without>, + mut trees: ResMut| { + let entity = trigger.entity; + + let Ok(proxy_key) = collider_query.get_mut(entity) else { + return; + }; + + let tree = trees.tree_for_type_mut(proxy_key.tree_type()); + + // Set sensor flag. + if let Some(proxy) = tree.get_proxy_mut(proxy_key.id()) { + proxy.flags.insert(ColliderTreeProxyFlags::SENSOR); + } + }, + ); + + // Case 8 + app.add_observer( + |trigger: On, + mut collider_query: Query<&ColliderTreeProxyKey, Without>, + mut trees: ResMut| { + let entity = trigger.entity; + + let Ok(proxy_key) = collider_query.get_mut(entity) else { + return; + }; + + let tree = trees.tree_for_type_mut(proxy_key.tree_type()); + + // Unset sensor flag. + if let Some(proxy) = tree.get_proxy_mut(proxy_key.id()) { + proxy.flags.remove(ColliderTreeProxyFlags::SENSOR); + } + }, + ); + + // Case 9 + app.add_observer( + |trigger: On, + mut collider_query: Query< + (&ColliderTreeProxyKey, Option<&CollisionLayers>), + Without, + >, + mut trees: ResMut| { + let entity = trigger.entity; + + let Ok((proxy_key, layers)) = collider_query.get_mut(entity) else { + return; + }; + + let tree = trees.tree_for_type_mut(proxy_key.tree_type()); + + // Update layers. + if let Some(proxy) = tree.get_proxy_mut(proxy_key.id()) { + proxy.layers = layers.copied().unwrap_or_default(); + } + }, + ); + + // Case 10 + app.add_observer( + |trigger: On, + mut collider_query: Query< + (&ColliderTreeProxyKey, Option<&ActiveCollisionHooks>), + Without, + >, + mut trees: ResMut| { + let entity = trigger.entity; + + let Ok((proxy_key, active_hooks)) = collider_query.get_mut(entity) else { + return; + }; + + let tree = trees.tree_for_type_mut(proxy_key.tree_type()); + + // Update active hooks flags. + if let Some(proxy) = tree.get_proxy_mut(proxy_key.id()) { + proxy.flags.set( + ColliderTreeProxyFlags::CUSTOM_FILTER, + active_hooks + .is_some_and(|h| h.contains(ActiveCollisionHooks::FILTER_PAIRS)), + ); + } + }, + ); + + // Case 11 + app.add_observer( + |trigger: On, + body_query: Query<(&RigidBodyColliders, Has)>, + mut collider_query: Query<&ColliderTreeProxyKey, Without>, + mut trees: ResMut| { + let entity = trigger.entity; + + let Ok((body_colliders, is_body_disabled)) = body_query.get(entity) else { + return; + }; + + for collider_entity in body_colliders.iter() { + let Ok(proxy_key) = collider_query.get_mut(collider_entity) else { + continue; + }; + + let tree = trees.tree_for_type_mut(proxy_key.tree_type()); + + // Update body disabled flag. + if let Some(proxy) = tree.get_proxy_mut(proxy_key.id()) { + proxy + .flags + .set(ColliderTreeProxyFlags::BODY_DISABLED, is_body_disabled); + } + } + }, + ); + } +} + +/// Adds a collider to the appropriate collider tree when the event `E` is triggered. +fn add_to_tree_on( + trigger: On, + body_query: Query<(&RigidBody, Has), Allow>, + mut collider_query: Query< + ( + Option<&ColliderOf>, + &ColliderAabb, + &EnlargedAabb, + &mut ColliderTreeProxyKey, + Option<&CollisionLayers>, + Has, + Has, + Option<&ActiveCollisionHooks>, + ), + F, + >, + mut trees: ResMut, + mut moved_proxies: ResMut, +) { + let entity = trigger.event_target(); + + let Ok(( + collider_of, + collider_aabb, + enlarged_aabb, + mut proxy_key, + layers, + is_sensor, + has_contact_events, + active_hooks, + )) = collider_query.get_mut(entity) + else { + return; + }; + + let (tree_type, is_body_disabled) = + if let Some(Ok((rb, disabled))) = collider_of.map(|c| body_query.get(c.body)) { + (ColliderTreeType::from_body(Some(*rb)), disabled) + } else { + (ColliderTreeType::Standalone, false) + }; + + let aabb = Aabb::from(*collider_aabb); + let enlarged_aabb = Aabb::from(enlarged_aabb.get()); + + let proxy = ColliderTreeProxy { + collider: entity, + body: collider_of.map(|c| c.body), + layers: layers.copied().unwrap_or_default(), + aabb, + flags: ColliderTreeProxyFlags::new( + is_sensor, + is_body_disabled, + has_contact_events, + active_hooks.copied().unwrap_or_default(), + ), + }; + + // Remove the old proxy if it exists. + if *proxy_key != ColliderTreeProxyKey::PLACEHOLDER { + let old_tree_type = proxy_key.tree_type(); + let old_tree = trees.tree_for_type_mut(old_tree_type); + old_tree.remove_proxy(proxy_key.id()); + moved_proxies.remove(&proxy_key); + } + + // Insert the proxy into the appropriate tree. + let tree = trees.tree_for_type_mut(tree_type); + let proxy_id = tree.add_proxy(enlarged_aabb, proxy); + + // Store the proxy key. + *proxy_key = ColliderTreeProxyKey::new(proxy_id, tree_type); + + // Mark the proxy as moved. + moved_proxies.insert(*proxy_key); +} + +/// Removes a collider from its collider tree when the event `E` is triggered. +fn remove_from_tree_on( + trigger: On, + mut collider_query: Query<&mut ColliderTreeProxyKey, F>, + mut trees: ResMut, + mut moved_proxies: ResMut, +) { + let entity = trigger.event_target(); + + let Ok(mut proxy_key) = collider_query.get_mut(entity) else { + return; + }; + + if *proxy_key == ColliderTreeProxyKey::PLACEHOLDER { + return; + } + + // Remove the proxy from its current tree. + let tree = trees.tree_for_type_mut(proxy_key.tree_type()); + tree.remove_proxy(proxy_key.id()); + moved_proxies.remove(&proxy_key); + + // Invalidate the proxy key. + *proxy_key = ColliderTreeProxyKey::PLACEHOLDER; +} + +/// A resource for tracking the last system change tick +/// when dynamic or kinematic collider AABBs were updated. +#[derive(Resource, Default)] +struct LastDynamicKinematicAabbUpdate(Tick); + +/// A resource for tracking moved proxies. +/// +/// Moved proxies are those whose [`ColliderAabb`] has moved outside of their +/// previous [`EnlargedAabb`], or whose collider has been added to a [`ColliderTree`]. +/// +/// [`ColliderTree`]: crate::collider_tree::ColliderTree +#[derive(Resource, Default)] +pub struct MovedProxies { + /// A vector of moved proxy keys. + proxies: Vec, + /// A set of moved proxy keys for quick lookup. + set: HashSet, +} + +impl MovedProxies { + /// Returns the keys of the moved proxies. + /// + /// The order of the keys is the order in which they were inserted. + #[inline] + pub fn proxies(&self) -> &[ColliderTreeProxyKey] { + &self.proxies + } + + /// Returns `true` if the proxy with the given key has moved. + #[inline] + pub fn contains(&self, proxy_key: ColliderTreeProxyKey) -> bool { + self.set.contains(&proxy_key) + } + + /// Inserts a moved proxy key. + /// + /// If the proxy key is already present, it is not added again. + #[inline] + pub fn insert(&mut self, proxy_key: ColliderTreeProxyKey) { + if self.set.insert(proxy_key) { + self.proxies.push(proxy_key); + } + } + + /// Removes a moved proxy key. This uses a linear search, + /// and may change the order of the remaining keys. + /// + /// If the proxy key is not present, nothing happens. + #[inline] + pub fn remove(&mut self, proxy_key: &ColliderTreeProxyKey) { + if self.set.remove(proxy_key) + && let Some(pos) = self.proxies.iter().position(|k| k == proxy_key) + { + self.proxies.swap_remove(pos); + } + } + + /// Clears the moved proxies. + #[inline] + pub fn clear(&mut self) { + self.proxies.clear(); + self.set.clear(); + } +} + +/// Bit vectors for tracking dynamic and kinematic proxies whose +/// [`ColliderAabb`] has moved outside of the previous [`EnlargedAabb`]. +/// +/// Set bits indicate [`ProxyId`]s of moved proxies. +#[derive(Resource, Default)] +struct EnlargedProxies { + bit_vec: EnlargedProxiesBitVec, + thread_local_bit_vec: ThreadLocal>, +} + +/// Bit vectors for tracking moved dynamic and kinematic proxies. +/// +/// Set bits indicate [`ProxyId`]s of moved proxies. +/// +/// [`ProxyId`]: crate::collider_tree::ProxyId +#[derive(Default)] +struct EnlargedProxiesBitVec { + // Note: Box2D indexes by shape ID, so it only needs one bit vector. + // In our case, we would instead index by entity ID, but this would + // require a potentially huge and very sparse bit vector since not + // all entities are colliders. So we use separate bit vectors for + // dynamic and kinematic bodies, and index by proxy ID instead. + dynamic: BitVec, + kinematic: BitVec, +} + +impl EnlargedProxies { + /// Clears the enlarged proxies and sets the capacity of the internal structures. + #[inline] + pub fn clear_and_set_capacity(&mut self, dynamic_capacity: usize, kinematic_capacity: usize) { + self.bit_vec + .dynamic + .set_bit_count_and_clear(dynamic_capacity); + self.bit_vec + .kinematic + .set_bit_count_and_clear(kinematic_capacity); + + self.thread_local_bit_vec.iter_mut().for_each(|context| { + let bit_vec_mut = &mut context.borrow_mut(); + bit_vec_mut + .dynamic + .set_bit_count_and_clear(dynamic_capacity); + bit_vec_mut + .kinematic + .set_bit_count_and_clear(kinematic_capacity); + }); + } + + /// Combines the thread-local enlarged proxy bit vectors into the main one. + #[inline] + pub fn combine_thread_local(&mut self) { + let bit_vec = &mut self.bit_vec; + self.thread_local_bit_vec.iter_mut().for_each(|context| { + let thread_local_bit_vec = context.borrow(); + bit_vec.dynamic.or(&thread_local_bit_vec.dynamic); + bit_vec.kinematic.or(&thread_local_bit_vec.kinematic); + }); + } +} + +/// Updates the AABBs of colliders attached to dynamic or kinematic rigid bodies. +// TODO: Optimize the change detection. +fn update_dynamic_kinematic_aabbs( + mut colliders: ParamSet<( + Query< + ( + Ref, + &mut ColliderAabb, + &mut EnlargedAabb, + &ColliderTreeProxyKey, + Ref, + Ref, + Option<&CollisionMargin>, + Option<&SpeculativeMargin>, + ), + Without, + >, + Query<(&ColliderAabb, &EnlargedAabb), Without>, + )>, + rb_query: Query< + ( + &Position, + &ComputedCenterOfMass, + &LinearVelocity, + &AngularVelocity, + &RigidBodyColliders, + Has, + ), + With, + >, + narrow_phase_config: Res, + length_unit: Res, + mut collider_trees: ResMut, + mut moved_proxies: ResMut, + mut enlarged_proxies: ResMut, + time: Res