653 lines
21 KiB
Rust
653 lines
21 KiB
Rust
#![allow(dead_code)]
|
|
|
|
use std::mem::swap;
|
|
|
|
use bevy::math::Vec3;
|
|
|
|
use crate::vvlib::intersections::aa_bounds_contains;
|
|
|
|
use super::constants::{sub_region, to_region_id};
|
|
|
|
use super::constants;
|
|
|
|
#[derive(Clone, Default, Debug)]
|
|
pub enum TreeNode<T> {
|
|
Branch(Box<[TreeNode<T>; 8]>),
|
|
Leaf(T),
|
|
#[default]
|
|
Empty,
|
|
}
|
|
|
|
#[derive(Debug)]
|
|
pub struct OctTree<T> {
|
|
pub size: usize,
|
|
pub center: Vec3,
|
|
pub(crate) root: TreeNode<T>,
|
|
pub needs_update: bool,
|
|
}
|
|
|
|
#[derive(Debug, Default, Clone, PartialEq, Eq)]
|
|
pub struct Path {
|
|
pub(crate) packed: u64,
|
|
pub(crate) length: usize,
|
|
}
|
|
impl Path {
|
|
pub fn pop(&mut self) -> Option<usize> {
|
|
if self.length == 0 {
|
|
return None;
|
|
}
|
|
self.length -= 1;
|
|
return Some(self.get_index(self.length));
|
|
}
|
|
pub fn parent(&self) -> Path {
|
|
let mut duplicate = self.clone();
|
|
duplicate.set_index(self.length - 1, 0);
|
|
duplicate.length = duplicate.length - 1;
|
|
return duplicate;
|
|
}
|
|
pub fn get_index(&self, index: usize) -> usize {
|
|
if self.length < index {
|
|
panic!("in too deep!");
|
|
}
|
|
let shift_amount = index as u64 * 3;
|
|
let unshifted_result = self.packed >> shift_amount;
|
|
(unshifted_result & 7) as usize
|
|
}
|
|
pub fn set_index(&mut self, index: usize, value: usize) {
|
|
if value > 7 {
|
|
panic!("cannot set to more than 7!");
|
|
}
|
|
if index + 1 > self.length {
|
|
self.length = index + 1;
|
|
}
|
|
let shift_amount = index as u64 * 3;
|
|
let shifted_value = (value as u64) << shift_amount;
|
|
let shift_mask = 7_u64 << shift_amount;
|
|
let shift_mask = !shift_mask;
|
|
self.packed = self.packed & shift_mask;
|
|
self.packed = self.packed | shifted_value;
|
|
}
|
|
pub fn append(&mut self, value: usize) {
|
|
self.set_index(self.length, value);
|
|
}
|
|
|
|
pub fn to_vec3(&self, center: Vec3, size: usize) -> Vec3 {
|
|
let mut iter = (center, size);
|
|
for i in 0..self.length {
|
|
iter = super::constants::sub_region(
|
|
iter.0,
|
|
iter.1,
|
|
self.get_index(i.try_into().unwrap()).try_into().unwrap(),
|
|
);
|
|
}
|
|
return iter.0;
|
|
}
|
|
}
|
|
|
|
impl<T: Clone> TreeNode<T> {
|
|
pub fn is_branch(&self) -> bool {
|
|
match self {
|
|
TreeNode::Branch(_) => true,
|
|
_ => false,
|
|
}
|
|
}
|
|
pub fn is_leaf(&self) -> bool {
|
|
match self {
|
|
TreeNode::Leaf(_) => true,
|
|
_ => false,
|
|
}
|
|
}
|
|
pub fn is_empty(&self) -> bool {
|
|
match self {
|
|
TreeNode::Empty => true,
|
|
_ => false,
|
|
}
|
|
}
|
|
pub fn branch_contents_reference(&self) -> Option<&[TreeNode<T>; 8]> {
|
|
match self {
|
|
TreeNode::Branch(a) => Some(a.as_ref()),
|
|
_ => None,
|
|
}
|
|
}
|
|
pub fn branch_contents_mutable(&mut self) -> Option<&mut [TreeNode<T>; 8]> {
|
|
match self {
|
|
TreeNode::Branch(a) => Some(a.as_mut()),
|
|
_ => None,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl<T: Clone> OctTree<T> {
|
|
pub fn new(npos: Vec3, value: T) -> Self {
|
|
Self {
|
|
size: 1,
|
|
center: npos,
|
|
root: TreeNode::Leaf(value),
|
|
needs_update: true,
|
|
}
|
|
}
|
|
|
|
pub fn remove_voxel(&mut self, location: Vec3) -> usize {
|
|
let result = self.voxel_at_location(location);
|
|
if result.is_some() {
|
|
let path = result.unwrap().1;
|
|
let mut count: usize = 0;
|
|
if path.length == 0 {
|
|
if self.root.is_leaf() {
|
|
self.root = TreeNode::Empty;
|
|
return 1;
|
|
}
|
|
}
|
|
let mut iter = &mut self.root;
|
|
for i in 0..path.length {
|
|
let region = path.get_index(i) as usize;
|
|
if let TreeNode::Branch(branch) = iter {
|
|
let arr = branch.as_mut();
|
|
iter = &mut arr[region];
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
*iter = TreeNode::Empty;
|
|
count += 1;
|
|
self.trim(&mut count);
|
|
self.needs_update = true;
|
|
return count;
|
|
} else {
|
|
return 0; // no culled leaves
|
|
}
|
|
}
|
|
fn has_only_one_element(subject: &mut [TreeNode<T>; 8]) -> Option<usize> {
|
|
// returns target location of the element
|
|
let mut res = Option::<usize>::None;
|
|
for element in subject.iter().enumerate() {
|
|
match element.1 {
|
|
TreeNode::Branch(_) | TreeNode::Leaf(_) => {
|
|
if res.is_none() {
|
|
res = Some(element.0);
|
|
} else {
|
|
return None;
|
|
}
|
|
}
|
|
_ => {}
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
fn trim(&mut self, removal_count: &mut usize) {
|
|
let count;
|
|
let mut extracted: &mut TreeNode<T> = &mut TreeNode::Empty;
|
|
match &mut self.root {
|
|
TreeNode::Branch(a) => {
|
|
let contents = a.as_mut();
|
|
count = Self::has_only_one_element(contents);
|
|
if count.is_some() {
|
|
extracted = &mut contents[count.unwrap()];
|
|
}
|
|
}
|
|
_ => return,
|
|
}
|
|
if count.is_some() {
|
|
self.root = extracted.clone();
|
|
let next_region =
|
|
constants::sub_region(self.center, self.size, count.unwrap().try_into().unwrap());
|
|
self.center = next_region.0;
|
|
self.size = next_region.1;
|
|
*removal_count += 1;
|
|
self.trim(removal_count);
|
|
}
|
|
}
|
|
|
|
pub fn set_voxel_at_path(&mut self, path: &Path, value: T) -> bool {
|
|
// return value is whether it was successful
|
|
let mut iter = &mut self.root;
|
|
if path.length == 0 {
|
|
match &mut self.root {
|
|
TreeNode::Leaf(nvalue) => {
|
|
*nvalue = value;
|
|
self.needs_update = true;
|
|
return true;
|
|
}
|
|
_ => {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
for i in 0..path.length {
|
|
let region = path.get_index(i) as usize;
|
|
match iter {
|
|
TreeNode::Branch(branch) => {
|
|
let arr = branch.as_mut();
|
|
iter = &mut arr[region];
|
|
}
|
|
_ => {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
match &mut iter {
|
|
TreeNode::Leaf(nvalue) => {
|
|
*nvalue = value;
|
|
self.needs_update = true;
|
|
return true;
|
|
}
|
|
_ => {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn set_voxel_at_location(&mut self, location: Vec3, value: T) -> Path {
|
|
// if the tree is empty, we want to fall out immediately - no looping needed.
|
|
match self.root {
|
|
TreeNode::Empty => {
|
|
self.center = location;
|
|
self.size = 1;
|
|
self.root = TreeNode::Leaf(value);
|
|
self.needs_update = true;
|
|
return Default::default();
|
|
}
|
|
_ => {} // continue to modify the tree structure - no easy answer yet
|
|
}
|
|
let mut path: Path = Default::default();
|
|
// if we don't already have a big enough non-empty tree, we need to branch upward till our tree contains our subject matter
|
|
if !aa_bounds_contains(self.center, self.size, location) {
|
|
let search_direction = constants::to_region_id(self.center, location);
|
|
|
|
while !aa_bounds_contains(self.center, self.size, location) {
|
|
match &self.root {
|
|
TreeNode::Branch(_) | TreeNode::Leaf(_) => {
|
|
let mut branch_extension: [TreeNode<T>; 8] = Default::default();
|
|
let (extension_center, extension_size) =
|
|
constants::parent_region(self.center, self.size, search_direction);
|
|
let slot_within_new_branch =
|
|
constants::to_region_id(extension_center, self.center);
|
|
let mut old: TreeNode<T> = TreeNode::Empty;
|
|
|
|
swap(&mut self.root, &mut old);
|
|
branch_extension[slot_within_new_branch as usize] = old;
|
|
self.root = TreeNode::Branch(Box::new(branch_extension));
|
|
self.center = extension_center;
|
|
self.size = extension_size;
|
|
match &self.root {
|
|
TreeNode::Branch(_) => {}
|
|
TreeNode::Leaf(_) => todo!(),
|
|
TreeNode::Empty => todo!(),
|
|
}
|
|
}
|
|
TreeNode::Empty => {
|
|
return Default::default();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// custom loop down: discover next position downward
|
|
let mut rc_center = self.center;
|
|
let mut rc_size = self.size;
|
|
let mut search_direction = constants::to_region_id(rc_center, location);
|
|
let mut rc_loc = &mut self.root;
|
|
|
|
while rc_size > 0 {
|
|
if search_direction != -1 {
|
|
let pindex = path.length;
|
|
path.set_index(pindex, search_direction as usize);
|
|
}
|
|
match rc_loc {
|
|
TreeNode::Branch(region) => {
|
|
let region = region.as_mut();
|
|
let next_rc_loc = &mut region[search_direction as usize];
|
|
(rc_center, rc_size) = sub_region(rc_center, rc_size, search_direction);
|
|
search_direction = constants::to_region_id(rc_center, location);
|
|
|
|
if rc_size == 1 {
|
|
let end = TreeNode::Leaf(value.clone());
|
|
*next_rc_loc = end;
|
|
self.needs_update = true;
|
|
return path;
|
|
}
|
|
match next_rc_loc {
|
|
TreeNode::Branch(_) => {}
|
|
TreeNode::Leaf(lvalue) => {
|
|
if rc_size != 1 {
|
|
return Default::default();
|
|
} else {
|
|
*lvalue = value.clone();
|
|
self.needs_update = true;
|
|
return path;
|
|
}
|
|
}
|
|
TreeNode::Empty => {
|
|
let branch_extension: [TreeNode<T>; 8] = Default::default();
|
|
let next_branch = TreeNode::Branch(Box::new(branch_extension));
|
|
*next_rc_loc = next_branch;
|
|
}
|
|
}
|
|
rc_loc = next_rc_loc;
|
|
}
|
|
TreeNode::Leaf(lvalue) => {
|
|
if rc_size == 1 {
|
|
*lvalue = value;
|
|
self.needs_update = true;
|
|
return path;
|
|
} else {
|
|
return Default::default();
|
|
}
|
|
}
|
|
TreeNode::Empty => {
|
|
return Default::default();
|
|
}
|
|
}
|
|
}
|
|
return Default::default();
|
|
}
|
|
|
|
pub fn collect_voxels(&self) -> Vec<(Path, Vec3, T)> {
|
|
let mut result = Vec::new();
|
|
Self::visitor(
|
|
&self.root,
|
|
self.center,
|
|
self.size,
|
|
&mut result,
|
|
Default::default(),
|
|
);
|
|
return result;
|
|
}
|
|
|
|
pub fn bounding_box(&self) -> Option<(Vec3, Vec3)> {
|
|
//center, size
|
|
let contents = self.collect_voxels();
|
|
let mut min: Option<Vec3> = None;
|
|
let mut max: Option<Vec3> = None;
|
|
for each in contents.iter() {
|
|
if min.is_none() {
|
|
min = Some(each.1);
|
|
} else {
|
|
let mut cont = min.unwrap();
|
|
if cont.x > each.1.x {
|
|
cont.x = each.1.x;
|
|
min = Some(cont);
|
|
}
|
|
if cont.y > each.1.y {
|
|
cont.y = each.1.y;
|
|
min = Some(cont);
|
|
}
|
|
if cont.z > each.1.z {
|
|
cont.z = each.1.z;
|
|
min = Some(cont);
|
|
}
|
|
}
|
|
if max.is_none() {
|
|
max = Some(each.1);
|
|
} else {
|
|
let mut cont: Vec3 = max.unwrap();
|
|
if cont.x < each.1.x {
|
|
cont.x = each.1.x;
|
|
max = Some(cont);
|
|
}
|
|
if cont.y < each.1.y {
|
|
cont.y = each.1.y;
|
|
max = Some(cont);
|
|
}
|
|
if cont.z < each.1.z {
|
|
cont.z = each.1.z;
|
|
max = Some(cont);
|
|
}
|
|
}
|
|
}
|
|
if max.is_none() && min.is_none() {
|
|
return None;
|
|
} else {
|
|
let center = min.unwrap() + max.unwrap();
|
|
let center = center / 2.;
|
|
let size = max.unwrap() - min.unwrap() + Vec3::ONE; // + 1 accounts for size of voxels themself, one half a voxel on each side
|
|
return Some((center, size));
|
|
}
|
|
}
|
|
|
|
fn visitor(
|
|
subject: &TreeNode<T>,
|
|
location: Vec3,
|
|
size: usize,
|
|
result: &mut Vec<(Path, Vec3, T)>,
|
|
path: Path,
|
|
) {
|
|
match subject {
|
|
TreeNode::Branch(branch) => {
|
|
let branch = branch.as_ref();
|
|
for (index, node) in branch.iter().enumerate() {
|
|
let (subpos, subsize) = sub_region(location, size, index as i32);
|
|
let mut path_next = path.clone();
|
|
path_next.set_index(path.length, index);
|
|
Self::visitor(node, subpos, subsize, result, path_next);
|
|
}
|
|
}
|
|
TreeNode::Leaf(value) => {
|
|
result.push((path, location, value.clone()));
|
|
}
|
|
TreeNode::Empty => {
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn voxel_at_path(&self, path: &Path) -> Option<T> {
|
|
// return value is whether it was successful
|
|
let mut iter = &self.root;
|
|
if path.length == 0 {
|
|
match &self.root {
|
|
TreeNode::Leaf(nvalue) => {
|
|
return Some(nvalue.clone());
|
|
}
|
|
_ => {
|
|
return None;
|
|
}
|
|
}
|
|
}
|
|
for i in 0..path.length {
|
|
let region = path.get_index(i) as usize;
|
|
match iter {
|
|
TreeNode::Branch(branch) => {
|
|
let arr = branch.as_ref();
|
|
iter = &arr[region];
|
|
}
|
|
_ => {
|
|
return None;
|
|
}
|
|
}
|
|
}
|
|
match &mut iter {
|
|
TreeNode::Leaf(nvalue) => {
|
|
return Some(nvalue.clone());
|
|
}
|
|
_ => {
|
|
return None;
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn voxel_at_location(&self, location: Vec3) -> Option<(T, Path)> {
|
|
if aa_bounds_contains(self.center, self.size, location) {
|
|
let mut rc_center = self.center;
|
|
let mut rc_size = self.size;
|
|
let mut path_result: Path = Default::default();
|
|
|
|
let mut search_direction = constants::to_region_id(rc_center, location);
|
|
|
|
let mut rc_loc = &self.root;
|
|
while rc_size > 1 {
|
|
if search_direction != -1 {
|
|
let pindex = path_result.length;
|
|
path_result.set_index(pindex, search_direction as usize);
|
|
}
|
|
match rc_loc {
|
|
TreeNode::Branch(branch) => {
|
|
if search_direction == -1 {
|
|
panic!("search direction gone wrong");
|
|
}
|
|
rc_loc = &branch[search_direction as usize];
|
|
(rc_center, rc_size) = sub_region(rc_center, rc_size, search_direction);
|
|
search_direction = to_region_id(rc_center, location);
|
|
}
|
|
TreeNode::Leaf(_) => {
|
|
panic!("tree not constructed correctly - non-unit voxel size");
|
|
}
|
|
TreeNode::Empty => {
|
|
return None; // does not exist.
|
|
}
|
|
}
|
|
}
|
|
match rc_loc {
|
|
TreeNode::Leaf(value) => {
|
|
return Some((value.clone(), path_result));
|
|
}
|
|
_ => {
|
|
return None;
|
|
}
|
|
}
|
|
}
|
|
None
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
pub fn test_path_return() {
|
|
let mut octtwee = OctTree::<String>::new(Vec3::ZERO, "a".to_string());
|
|
octtwee.set_voxel_at_location(Vec3::splat(1.), "test".to_string());
|
|
octtwee.set_voxel_at_location(Vec3::splat(19.), "test".to_string());
|
|
let res0 = octtwee.set_voxel_at_location(Vec3::new(190., 15., -100.), "test".to_string());
|
|
|
|
for i in 0..res0.length {
|
|
println!("{}", res0.get_index(i));
|
|
}
|
|
let res1 = octtwee.voxel_at_location(Vec3::new(190., 15., -100.));
|
|
match res1.clone() {
|
|
Some((_, c)) => {
|
|
assert_eq!(res0.packed, res1.clone().unwrap().1.packed); //internal path consistency with both functions
|
|
assert_eq!(
|
|
Vec3::new(190., 15., -100.),
|
|
c.to_vec3(octtwee.center, octtwee.size)
|
|
);
|
|
assert_eq!(res1.clone().unwrap().0, "test".to_string());
|
|
}
|
|
_ => {
|
|
panic!("should be something there!");
|
|
}
|
|
}
|
|
assert!(octtwee.set_voxel_at_path(&res1.clone().unwrap().1, "test_change".to_string())); //if we cant modify it we get false, a failed test
|
|
|
|
assert_eq!(
|
|
octtwee
|
|
.voxel_at_location(Vec3::new(190., 15., -100.))
|
|
.unwrap()
|
|
.0,
|
|
octtwee.voxel_at_path(&res1.clone().unwrap().1).unwrap()
|
|
);
|
|
let res1 = octtwee.voxel_at_location(Vec3::new(190., 15., -100.));
|
|
match &res1 {
|
|
Some((a, _)) => {
|
|
assert_eq!(*a, "test_change".to_string());
|
|
}
|
|
None => {
|
|
panic!("something should be here");
|
|
}
|
|
}
|
|
}
|
|
#[test]
|
|
pub fn test_bounding() {
|
|
let mut octtree = OctTree::<()>::new(Vec3::ZERO, ());
|
|
octtree.set_voxel_at_location(Vec3::splat(10.), ());
|
|
assert_eq!(
|
|
octtree.bounding_box().unwrap(),
|
|
(Vec3::splat(5.), Vec3::splat(11.))
|
|
);
|
|
}
|
|
#[test]
|
|
pub fn test_collect() {
|
|
let zeropos = Vec3::ZERO;
|
|
let otherpos = Vec3::new(10., -5., 1.);
|
|
let mut octtree = OctTree::<usize>::new(zeropos, 0);
|
|
octtree.set_voxel_at_location(otherpos, 27);
|
|
let x = octtree.collect_voxels();
|
|
assert_eq!(
|
|
x[0],
|
|
(
|
|
Path {
|
|
packed: 1170,
|
|
length: 4,
|
|
},
|
|
Vec3::ZERO,
|
|
0
|
|
)
|
|
);
|
|
assert_eq!(
|
|
x[1],
|
|
(
|
|
Path {
|
|
packed: 902,
|
|
length: 4,
|
|
},
|
|
otherpos,
|
|
27
|
|
)
|
|
);
|
|
}
|
|
#[test]
|
|
pub fn test_removal() {
|
|
let mut test_tree = OctTree::new(Vec3::ZERO, 0);
|
|
test_tree.set_voxel_at_location(Vec3::splat(10.), 5);
|
|
test_tree.remove_voxel(Vec3::ZERO);
|
|
assert_eq!(test_tree.size, 1);
|
|
}
|
|
|
|
#[test]
|
|
pub fn test_octtree() {
|
|
let origin = Vec3::ZERO;
|
|
let neighbor = Vec3::new(1., 1., 1.);
|
|
let mut test_oct = OctTree::<i32>::new(origin, 1);
|
|
test_oct.set_voxel_at_location(neighbor, 2);
|
|
|
|
let far_off_bullshit = Vec3::new(26., 26., 26.);
|
|
test_oct.set_voxel_at_location(far_off_bullshit, 5);
|
|
dbg!(&test_oct);
|
|
assert_eq!(test_oct.voxel_at_location(neighbor).unwrap().0, 2);
|
|
assert_eq!(test_oct.voxel_at_location(origin).unwrap().0, 1);
|
|
assert_eq!(test_oct.voxel_at_location(far_off_bullshit).unwrap().0, 5);
|
|
}
|
|
#[test]
|
|
pub fn test_path() {
|
|
let mut path_test: Path = Default::default();
|
|
path_test.set_index(0, 6);
|
|
path_test.set_index(7, 4);
|
|
assert_eq!(path_test.get_index(7), 4);
|
|
println!("{:#?}", dbg!(path_test.clone()));
|
|
assert_eq!(path_test.get_index(0), 6);
|
|
println!("{:#?}", dbg!(path_test.clone()));
|
|
}
|
|
#[test]
|
|
pub fn fuzztestpath() {
|
|
use rand::Rng;
|
|
let origin = Vec3::ZERO;
|
|
let mut test_oct = OctTree::<i32>::new(origin, 1);
|
|
let range = 10000.;
|
|
let mut rng = rand::thread_rng();
|
|
let mut list: Vec<(Vec3, i32)> = Vec::new();
|
|
|
|
for _ in 0..10000 {
|
|
let x: f32 = rng.gen_range(-range..range);
|
|
let y: f32 = rng.gen_range(-range..range);
|
|
let z: f32 = rng.gen_range(-range..range);
|
|
let w: i32 = rng.gen_range(0..range as i32);
|
|
let vec = Vec3::new(x, y, z);
|
|
list.push((vec, w));
|
|
test_oct.set_voxel_at_location(vec, w);
|
|
}
|
|
|
|
for item in list {
|
|
let get = test_oct.voxel_at_location(item.0);
|
|
assert_eq!(item.1, get.clone().unwrap().0);
|
|
let get = test_oct.voxel_at_path(&get.unwrap().1);
|
|
assert_eq!(item.1, get.unwrap());
|
|
}
|
|
}
|