vox_geometry_rust 0.1.2

/*
 * // Copyright (c) 2021 Feng Yang
 * //
 * // I am making my contributions/submissions to this project solely in my
 * // personal capacity and am not conveying any rights to any intellectual
 * // property of any third parties.
 */

use crate::usize3::USize3;
use crate::isize3::ISize3;
use crate::vector3::Vector3D;
use crate::point_neighbor_searcher3::*;
use std::sync::{RwLock, Arc};

///
/// # Hash grid-based 3-D point searcher.
///
/// This class implements 3-D point searcher by using hash grid for its internal
/// acceleration data structure. Each point is recorded to its corresponding
/// bucket where the hashing function is 3-D grid mapping.
///
pub struct PointHashGridSearcher3 {
    _grid_spacing: f64,
    _resolution: ISize3,
    _points: Vec<Vector3D>,
    _buckets: Vec<Vec<usize>>,
}

impl PointHashGridSearcher3 {
    pub fn new_default() -> PointHashGridSearcher3 {
        return PointHashGridSearcher3 {
            _grid_spacing: 1.0,
            _resolution: ISize3::new(1, 1, 1),
            _points: vec![],
            _buckets: vec![],
        };
    }

    ///
    /// ## Constructs hash grid with given resolution and grid spacing.
    ///
    /// This constructor takes hash grid resolution and its grid spacing as
    /// its input parameters. The grid spacing must be 3x or greater than
    /// search radius.
    ///
    /// - parameter:  resolution  The resolution.
    /// - parameter:  grid_spacing The grid spacing.
    ///
    pub fn new_vec(resolution: USize3, grid_spacing: f64) -> PointHashGridSearcher3 {
        return PointHashGridSearcher3::new(resolution.x,
                                           resolution.y,
                                           resolution.z, grid_spacing);
    }

    ///
    /// \brief      Constructs hash grid with given resolution and grid spacing.
    ///
    /// This constructor takes hash grid resolution and its grid spacing as
    /// its input parameters. The grid spacing must be 3x or greater than
    /// search radius.
    ///
    /// - parameter:  resolution_x The resolution x.
    /// - parameter:  resolution_y The resolution y.
    /// - parameter:  grid_spacing The grid spacing.
    ///
    pub fn new(resolution_x: usize,
               resolution_y: usize,
               resolution_z: usize,
               grid_spacing: f64) -> PointHashGridSearcher3 {
        return PointHashGridSearcher3 {
            _grid_spacing: grid_spacing,
            _resolution: ISize3::new(resolution_x.max(1) as isize,
                                     resolution_y.max(1) as isize,
                                     resolution_z.max(1) as isize),
            _points: vec![],
            _buckets: vec![],
        };
    }

    /// Returns builder fox PointHashGridSearcher3.
    pub fn builder() -> Builder {
        return Builder::new();
    }

    ///
    /// \brief      Creates a new instance of the object with same properties
    ///             than original.
    ///
    /// \return     Copy of this object.
    ///
    pub fn clone(&self) -> PointHashGridSearcher3Ptr {
        let mut searcher = PointHashGridSearcher3::new_default();
        searcher.set(self);
        return PointHashGridSearcher3Ptr::new(RwLock::new(searcher));
    }

    /// Copy from the other instance.
    pub fn set(&mut self, other: &PointHashGridSearcher3) {
        self._grid_spacing = other._grid_spacing;
        self._resolution = other._resolution;
        self._points = other._points.clone();
        self._buckets = other._buckets.clone();
    }
}

impl PointHashGridSearcher3 {
    ///
    /// ##  Adds a single point to the hash grid.
    ///
    /// This function adds a single point to the hash grid for future queries.
    /// It can be used for a hash grid that is already built by calling function
    /// PointHashGridSearcher3::build.
    ///
    /// - parameter:  point The point to be added.
    ///
    pub fn add(&mut self, point: Vector3D) {
        if self._buckets.is_empty() {
            let arr = vec![point];
            self.build(&arr);
        } else {
            let i = self._points.len();
            self._points.push(point);
            let key = self.get_hash_key_from_position(&point);
            self._buckets[key].push(i);
        }
    }

    ///
    /// ##  Returns the internal bucket.
    ///
    /// A bucket is a list of point indices that has same hash value. This
    /// function returns the (immutable) internal bucket structure.
    ///
    /// \return     List of buckets.
    ///
    pub fn buckets(&self) -> &Vec<Vec<usize>> {
        return &self._buckets;
    }

    ///
    /// ## Returns the hash value for given 3-D bucket index.
    ///
    /// - parameter:  bucket_index The bucket index.
    ///
    /// \return     The hash key from bucket index.
    ///
    pub fn get_hash_key_from_bucket_index(&self, bucket_index: &ISize3) -> usize {
        let mut wrapped_index = *bucket_index;
        wrapped_index.x = bucket_index.x % self._resolution.x;
        wrapped_index.y = bucket_index.y % self._resolution.y;
        wrapped_index.z = bucket_index.z % self._resolution.z;
        if wrapped_index.x < 0 {
            wrapped_index.x += self._resolution.x;
        }
        if wrapped_index.y < 0 {
            wrapped_index.y += self._resolution.y;
        }
        if wrapped_index.z < 0 {
            wrapped_index.z += self._resolution.z;
        }
        return ((wrapped_index.z * self._resolution.y + wrapped_index.y) * self._resolution.x
            + wrapped_index.x) as usize;
    }

    ///
    /// ## Gets the bucket index from a point.
    ///
    /// - parameter:  position The position of the point.
    ///
    /// \return     The bucket index.
    ///
    pub fn get_bucket_index(&self, position: &Vector3D) -> ISize3 {
        let mut bucket_index = ISize3::new_default();
        bucket_index.x = f64::floor(position.x / self._grid_spacing) as isize;
        bucket_index.y = f64::floor(position.y / self._grid_spacing) as isize;
        bucket_index.z = f64::floor(position.z / self._grid_spacing) as isize;
        return bucket_index;
    }

    fn get_hash_key_from_position(&self, position: &Vector3D) -> usize {
        let bucket_index = self.get_bucket_index(position);

        return self.get_hash_key_from_bucket_index(&bucket_index);
    }

    fn get_nearby_keys(&self, position: &Vector3D, nearby_keys: &mut [usize; 8]) {
        let origin_index = self.get_bucket_index(position);
        let mut nearby_bucket_indices: [ISize3; 8] = [ISize3::new_default(); 8];

        for i in 0..8 {
            nearby_bucket_indices[i] = origin_index;
        }

        if (origin_index.x as f64 + 0.5) * self._grid_spacing <= position.x {
            nearby_bucket_indices[4].x += 1;
            nearby_bucket_indices[5].x += 1;
            nearby_bucket_indices[6].x += 1;
            nearby_bucket_indices[7].x += 1;
        } else {
            nearby_bucket_indices[4].x -= 1;
            nearby_bucket_indices[5].x -= 1;
            nearby_bucket_indices[6].x -= 1;
            nearby_bucket_indices[7].x -= 1;
        }

        if (origin_index.y as f64 + 0.5) * self._grid_spacing <= position.y {
            nearby_bucket_indices[2].y += 1;
            nearby_bucket_indices[3].y += 1;
            nearby_bucket_indices[6].y += 1;
            nearby_bucket_indices[7].y += 1;
        } else {
            nearby_bucket_indices[2].y -= 1;
            nearby_bucket_indices[3].y -= 1;
            nearby_bucket_indices[6].y -= 1;
            nearby_bucket_indices[7].y -= 1;
        }

        if (origin_index.z as f64 + 0.5) * self._grid_spacing <= position.z {
            nearby_bucket_indices[1].z += 1;
            nearby_bucket_indices[3].z += 1;
            nearby_bucket_indices[5].z += 1;
            nearby_bucket_indices[7].z += 1;
        } else {
            nearby_bucket_indices[1].z -= 1;
            nearby_bucket_indices[3].z -= 1;
            nearby_bucket_indices[5].z -= 1;
            nearby_bucket_indices[7].z -= 1;
        }

        for i in 0..8 {
            nearby_keys[i] = self.get_hash_key_from_bucket_index(&nearby_bucket_indices[i]);
        }
    }
}

impl PointNeighborSearcher3 for PointHashGridSearcher3 {
    fn type_name() -> String {
        return "PointHashGridSearcher3".parse().unwrap();
    }

    fn build(&mut self, points: &Vec<Vector3D>) {
        self._buckets.clear();
        self._points.clear();

        // Allocate memory chunks
        self._buckets.resize(self._resolution.x as usize
                                 * self._resolution.y as usize
                                 * self._resolution.z as usize, Vec::new());
        self._points.resize(points.len(), Vector3D::new_default());

        if points.is_empty() {
            return;
        }

        // Put points into buckets
        for i in 0..points.len() {
            self._points[i] = points[i];
            let key = self.get_hash_key_from_position(&points[i]);
            self._buckets[key].push(i);
        }
    }

    fn for_each_nearby_point<Callback>(&self, origin: &Vector3D, radius: f64, callback: &mut Callback)
        where Callback: ForEachNearbyPointFunc {
        if self._buckets.is_empty() {
            return;
        }

        let mut nearby_keys: [usize; 8] = [0; 8];
        self.get_nearby_keys(origin, &mut nearby_keys);

        let query_radius_squared = radius * radius;

        for i in 0..8 {
            let bucket = &self._buckets[nearby_keys[i]];
            let number_of_points_in_bucket = bucket.len();

            for j in 0..number_of_points_in_bucket {
                let point_index = bucket[j];
                let r_squared = (self._points[point_index] - *origin).length_squared();
                if r_squared <= query_radius_squared {
                    callback(point_index, &self._points[point_index]);
                }
            }
        }
    }

    fn has_nearby_point(&self, origin: &Vector3D, radius: f64) -> bool {
        if self._buckets.is_empty() {
            return false;
        }

        let mut nearby_keys: [usize; 8] = [0; 8];
        self.get_nearby_keys(origin, &mut nearby_keys);

        let query_radius_squared = radius * radius;

        for i in 0..8 {
            let bucket = &self._buckets[nearby_keys[i]];
            let number_of_points_in_bucket = bucket.len();

            for j in 0..number_of_points_in_bucket {
                let point_index = bucket[j];
                let r_squared = (self._points[point_index] - *origin).length_squared();
                if r_squared <= query_radius_squared {
                    return true;
                }
            }
        }

        return false;
    }
}

/// Shared pointer for the PointHashGridSearcher3 type.
pub type PointHashGridSearcher3Ptr = Arc<RwLock<PointHashGridSearcher3>>;

///
/// # Front-end to create PointHashGridSearcher3 objects step by step.
///
pub struct Builder {
    _resolution: USize3,
    _grid_spacing: f64,
}

impl Builder {
    /// Returns builder with resolution.
    pub fn with_resolution(&mut self, resolution: USize3) -> &mut Self {
        self._resolution = resolution;
        return self;
    }

    /// Returns builder with grid spacing.
    pub fn with_grid_spacing(&mut self, grid_spacing: f64) -> &mut Self {
        self._grid_spacing = grid_spacing;
        return self;
    }

    /// Builds PointHashGridSearcher3 instance.
    pub fn build(&mut self) -> PointHashGridSearcher3 {
        return PointHashGridSearcher3::new_vec(self._resolution, self._grid_spacing);
    }

    /// Builds shared pointer of PointHashGridSearcher3 instance.
    pub fn make_shared(&mut self) -> PointHashGridSearcher3Ptr {
        return PointHashGridSearcher3Ptr::new(RwLock::new(self.build()));
    }

    /// constructor
    pub fn new() -> Builder {
        return Builder {
            _resolution: USize3::new(64, 64, 64),
            _grid_spacing: 1.0,
        };
    }
}