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::usize2::USize2;
use crate::isize2::ISize2;
use crate::vector2::Vector2D;
use crate::point_neighbor_searcher2::*;
use std::sync::{RwLock, Arc};

///
/// # Hash grid-based 2-D point searcher.
///
/// This class implements 2-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 2-D grid mapping.
///
pub struct PointHashGridSearcher2 {
    _grid_spacing: f64,
    _resolution: ISize2,
    _points: Vec<Vector2D>,
    _buckets: Vec<Vec<usize>>,
}

impl PointHashGridSearcher2 {
    pub fn new_default() -> PointHashGridSearcher2 {
        return PointHashGridSearcher2 {
            _grid_spacing: 1.0,
            _resolution: ISize2::new(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 2x or greater than
    /// search radius.
    ///
    /// - parameter:  resolution  The resolution.
    /// - parameter:  grid_spacing The grid spacing.
    ///
    pub fn new_vec(resolution: USize2, grid_spacing: f64) -> PointHashGridSearcher2 {
        return PointHashGridSearcher2::new(resolution.x, resolution.y, 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 2x 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,
               grid_spacing: f64) -> PointHashGridSearcher2 {
        return PointHashGridSearcher2 {
            _grid_spacing: grid_spacing,
            _resolution: ISize2::new(resolution_x.max(1) as isize, resolution_y.max(1) as isize),
            _points: vec![],
            _buckets: vec![],
        };
    }

    /// Returns builder fox PointHashGridSearcher2.
    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) -> PointHashGridSearcher2Ptr {
        let mut searcher = PointHashGridSearcher2::new_default();
        searcher.set(self);
        return PointHashGridSearcher2Ptr::new(RwLock::new(searcher));
    }

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

impl PointHashGridSearcher2 {
    ///
    /// ##  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
    /// PointHashGridSearcher2::build.
    ///
    /// - parameter:  point The point to be added.
    ///
    pub fn add(&mut self, point: Vector2D) {
        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 2-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: &ISize2) -> 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;
        if wrapped_index.x < 0 {
            wrapped_index.x += self._resolution.x;
        }
        if wrapped_index.y < 0 {
            wrapped_index.y += self._resolution.y;
        }
        return (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: &Vector2D) -> ISize2 {
        let mut bucket_index = ISize2::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;
        return bucket_index;
    }

    fn get_hash_key_from_position(&self, position: &Vector2D) -> 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: &Vector2D, nearby_keys: &mut [usize; 4]) {
        let origin_index = self.get_bucket_index(position);
        let mut nearby_bucket_indices: [ISize2; 4] = [ISize2::new_default(); 4];

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

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

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

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

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

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

        // Allocate memory chunks
        self._buckets.resize(self._resolution.x as usize * self._resolution.y as usize, Vec::new());
        self._points.resize(points.len(), Vector2D::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: &Vector2D, radius: f64, callback: &mut Callback)
        where Callback: ForEachNearbyPointFunc {
        if self._buckets.is_empty() {
            return;
        }

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

        let query_radius_squared = radius * radius;

        for i in 0..4 {
            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: &Vector2D, radius: f64) -> bool {
        if self._buckets.is_empty() {
            return false;
        }

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

        let query_radius_squared = radius * radius;

        for i in 0..4 {
            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 PointHashGridSearcher2 type.
pub type PointHashGridSearcher2Ptr = Arc<RwLock<PointHashGridSearcher2>>;

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

impl Builder {
    /// Returns builder with resolution.
    pub fn with_resolution(&mut self, resolution: USize2) -> &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 PointHashGridSearcher2 instance.
    pub fn build(&mut self) -> PointHashGridSearcher2 {
        return PointHashGridSearcher2::new_vec(self._resolution, self._grid_spacing);
    }

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

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