use crate::bounding_volume::{Aabb, SimdAabb};
use crate::math::{Real, Vector};
use na::SimdValue;
#[cfg(feature = "rkyv")]
use rkyv::{bytecheck, CheckBytes};
pub trait IndexedData: Copy {
fn default() -> Self;
fn index(&self) -> usize;
}
impl IndexedData for usize {
fn default() -> Self {
u32::MAX as usize
}
fn index(&self) -> usize {
*self
}
}
impl IndexedData for u32 {
fn default() -> Self {
u32::MAX
}
fn index(&self) -> usize {
*self as usize
}
}
impl IndexedData for u64 {
fn default() -> Self {
u64::MAX
}
fn index(&self) -> usize {
*self as usize
}
}
pub type SimdNodeIndex = u32;
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[cfg_attr(
feature = "rkyv",
derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes),
archive(as = "Self")
)]
pub struct NodeIndex {
pub index: SimdNodeIndex, pub lane: u8, }
impl NodeIndex {
pub(super) fn new(index: u32, lane: u8) -> Self {
Self { index, lane }
}
pub(super) fn invalid() -> Self {
Self {
index: u32::MAX,
lane: 0,
}
}
pub(super) fn is_invalid(&self) -> bool {
self.index == u32::MAX
}
}
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[cfg_attr(
feature = "rkyv",
derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize),
archive(as = "Self")
)]
#[derive(Clone, Copy, Debug, Default, Eq, Hash, Ord, PartialEq, PartialOrd)]
pub struct QbvhNodeFlags(u8);
bitflags::bitflags! {
impl QbvhNodeFlags: u8 {
const LEAF = 0b0001;
const CHANGED = 0b0010;
const DIRTY = 0b0100;
}
}
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[cfg_attr(
feature = "rkyv",
derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize),
archive(check_bytes)
)]
pub struct QbvhNode {
pub simd_aabb: SimdAabb,
pub children: [u32; 4],
pub parent: NodeIndex,
pub flags: QbvhNodeFlags,
}
impl QbvhNode {
#[inline]
pub fn is_leaf(&self) -> bool {
self.flags.contains(QbvhNodeFlags::LEAF)
}
#[inline]
pub fn is_dirty(&self) -> bool {
self.flags.contains(QbvhNodeFlags::DIRTY)
}
#[inline]
pub fn set_dirty(&mut self, dirty: bool) {
self.flags.set(QbvhNodeFlags::DIRTY, dirty);
}
#[inline]
pub fn is_changed(&self) -> bool {
self.flags.contains(QbvhNodeFlags::CHANGED)
}
#[inline]
pub fn set_changed(&mut self, changed: bool) {
self.flags.set(QbvhNodeFlags::CHANGED, changed);
}
#[inline]
pub fn empty() -> Self {
Self {
simd_aabb: SimdAabb::new_invalid(),
children: [u32::MAX; 4],
parent: NodeIndex::invalid(),
flags: QbvhNodeFlags::default(),
}
}
#[inline]
pub fn empty_leaf_with_parent(parent: NodeIndex) -> Self {
Self {
simd_aabb: SimdAabb::new_invalid(),
children: [u32::MAX; 4],
parent,
flags: QbvhNodeFlags::LEAF,
}
}
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[cfg_attr(
feature = "rkyv",
derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize),
archive(check_bytes)
)]
pub struct QbvhProxy<LeafData> {
pub node: NodeIndex,
pub data: LeafData, }
impl<LeafData> QbvhProxy<LeafData> {
pub(super) fn invalid() -> Self
where
LeafData: IndexedData,
{
Self {
node: NodeIndex::invalid(),
data: LeafData::default(),
}
}
pub(super) fn detached(data: LeafData) -> Self {
Self {
node: NodeIndex::invalid(),
data,
}
}
pub(super) fn is_detached(&self) -> bool {
self.node.is_invalid()
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[cfg_attr(
feature = "rkyv",
derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize),
archive(check_bytes)
)]
#[repr(C)]
#[derive(Debug, Clone)]
pub struct Qbvh<LeafData> {
pub(super) root_aabb: Aabb,
pub(super) nodes: Vec<QbvhNode>,
pub(super) dirty_nodes: Vec<u32>,
pub(super) free_list: Vec<u32>,
pub(super) proxies: Vec<QbvhProxy<LeafData>>,
}
impl<LeafData: IndexedData> Default for Qbvh<LeafData> {
fn default() -> Self {
Self::new()
}
}
impl<LeafData: IndexedData> Qbvh<LeafData> {
pub fn new() -> Self {
Qbvh {
root_aabb: Aabb::new_invalid(),
nodes: Vec::new(),
dirty_nodes: Vec::new(),
free_list: Vec::new(),
proxies: Vec::new(),
}
}
pub fn iter_data_mut(&mut self) -> impl Iterator<Item = (NodeIndex, &mut LeafData)> {
self.proxies.iter_mut().map(|p| (p.node, &mut p.data))
}
pub fn iter_data(&self) -> impl Iterator<Item = (NodeIndex, &LeafData)> {
self.proxies.iter().map(|p| (p.node, &p.data))
}
pub fn node_aabb(&self, node_id: NodeIndex) -> Option<Aabb> {
self.nodes
.get(node_id.index as usize)
.map(|n| n.simd_aabb.extract(node_id.lane as usize))
}
pub fn leaf_data(&self, node_id: NodeIndex) -> Option<LeafData> {
let node = self.nodes.get(node_id.index as usize)?;
if !node.is_leaf() {
return None;
}
let proxy = self
.proxies
.get(node.children[node_id.lane as usize] as usize)?;
Some(proxy.data)
}
pub fn raw_nodes(&self) -> &[QbvhNode] {
&self.nodes
}
pub fn raw_proxies(&self) -> &[QbvhProxy<LeafData>] {
&self.proxies
}
pub fn scaled(mut self, scale: &Vector<Real>) -> Self {
self.root_aabb = self.root_aabb.scaled(scale);
for node in &mut self.nodes {
node.simd_aabb = node.simd_aabb.scaled(&Vector::splat(*scale));
}
self
}
}
impl<LeafData: IndexedData> Qbvh<LeafData> {
pub fn root_aabb(&self) -> &Aabb {
&self.root_aabb
}
}
#[cfg(test)]
mod test {
use crate::bounding_volume::Aabb;
use crate::math::{Point, Vector};
use crate::partitioning::Qbvh;
#[test]
fn multiple_identical_aabb_stack_overflow() {
let aabb = Aabb::new(Point::origin(), Vector::repeat(1.0).into());
for k in 0u32..20 {
let mut tree = Qbvh::new();
tree.clear_and_rebuild((0..k).map(|i| (i, aabb)), 0.0);
}
}
}