rust_3d/
oc_tree.rs

1/*
2Copyright 2016 Martin Buck
3
4Permission is hereby granted, free of charge, to any person obtaining a copy
5of this software and associated documentation files (the "Software"),
6to deal in the Software without restriction, including without limitation the
7rights to use, copy, modify, merge, publish, distribute, sublicense,
8and/or sell copies of the Software, and to permit persons to whom the Software
9is furnished to do so, subject to the following conditions:
10
11The above copyright notice and this permission notice shall
12be included all copies or substantial portions of the Software.
13
14THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
15EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
16MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
17IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
18DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
19TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE
20OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
21*/
22
23//! OcTree https://en.wikipedia.org/wiki/Octree
24
25use std::{collections::HashSet, iter::IntoIterator};
26
27use crate::*;
28
29//------------------------------------------------------------------------------
30
31#[derive(Default, Debug, PartialEq, PartialOrd, Ord, Eq, Clone, Hash)]
32/// OcTree https://en.wikipedia.org/wiki/Octree
33pub struct OcTree<P>
34where
35    P: Is3D,
36{
37    root: Option<OcNode<P>>,
38    bb: BoundingBox3D,
39}
40
41impl<P> IsTree3D<P> for OcTree<P>
42where
43    P: IsBuildable3D + Clone + Default,
44{
45    fn size(&self) -> usize {
46        match self.root {
47            None => 0,
48            Some(ref node) => node.size(),
49        }
50    }
51
52    fn to_pointcloud(&self) -> PointCloud3D<P> {
53        self.collect(-1)
54    }
55
56    fn build(&mut self, pc: PointCloud3D<P>) -> Result<()> {
57        self.bb = pc.bounding_box_maybe()?;
58        let mut unique_data = Vec::new();
59        let mut set = HashSet::new();
60        for p in pc.data {
61            set.insert(p);
62        }
63
64        unique_data.extend(set.into_iter());
65        self.root = Some(OcNode::new(&self.bb, unique_data)?);
66
67        Ok(())
68    }
69}
70
71impl<P> IsOcTree<P> for OcTree<P>
72where
73    P: IsBuildable3D + Clone + Default,
74{
75    //@todo rewrite or make new method which returns cog instead of stopping recursion
76    fn collect(&self, maxdepth: i8) -> PointCloud3D<P> {
77        let mut result = PointCloud3D::new();
78        if let Some(ref node) = self.root {
79            node.collect(0, maxdepth, &mut result);
80        }
81        result
82    }
83}
84
85#[derive(Debug, PartialEq, PartialOrd, Ord, Eq, Clone, Hash)]
86/// OcNode, which is a single node used within OcTree
87enum OcNode<P>
88where
89    P: Is3D,
90{
91    Leaf(P),
92    Node(Internal<P>),
93}
94
95#[derive(Default, Debug, PartialEq, PartialOrd, Ord, Eq, Clone, Hash)]
96struct Internal<P>
97where
98    P: Is3D,
99{
100    // naming : p == positive, n == negative ||| xyz   => pnp => x positive, y negative, z positive direction from center
101    ppp: Option<Box<OcNode<P>>>,
102    ppn: Option<Box<OcNode<P>>>,
103    pnp: Option<Box<OcNode<P>>>,
104    pnn: Option<Box<OcNode<P>>>,
105    npp: Option<Box<OcNode<P>>>,
106    npn: Option<Box<OcNode<P>>>,
107    nnp: Option<Box<OcNode<P>>>,
108    nnn: Option<Box<OcNode<P>>>,
109}
110
111enum Direction {
112    PPP,
113    PPN,
114    PNP,
115    PNN,
116    NPP,
117    NPN,
118    NNP,
119    NNN,
120}
121
122/*
123/// Calculates the direction of one point to another in terms of an enum
124pub fn calc_direction<P>(reference: &Point3D, p: &Point3D) -> Direction where
125    P: Is3D {
126
127    if p.x() >= reference.x() && p.y() >= reference.y() && p.z() >= reference.z() {
128        Direction::PPP
129    } else if p.x() >= reference.x() && p.y() >= reference.y() && p.z() < reference.z() {
130        Direction::PPN
131    } else if p.x() >= reference.x() && p.y() < reference.y() && p.z() >= reference.z() {
132        Direction::PNP
133    } else if p.x() >= reference.x() && p.y() < reference.y() && p.z() < reference.z() {
134        Direction::PNN
135    } else if p.x() < reference.x() && p.y() >= reference.y() && p.z() >= reference.z() {
136        Direction::NPP
137    } else if p.x() < reference.x() && p.y() >= reference.y() && p.z() < reference.z() {
138        Direction::NPN
139    } else if p.x() >= reference.x() && p.y() < reference.y() && p.z() >= reference.z() {
140        Direction::NNP
141    } else { //if p.x() < reference.x() && p.y() < reference.y() && p.z() < reference.z() {
142        Direction::NNN
143    }
144}
145*/
146
147impl<P> OcNode<P>
148where
149    P: Is3D,
150{
151    /// Returns the size of the oc node
152    pub fn size(&self) -> usize {
153        match self {
154            Self::Leaf(_) => 1,
155            Self::Node(internal) => {
156                let mut result: usize = 0;
157                if let Some(ref n) = internal.ppp {
158                    result += n.size();
159                }
160                if let Some(ref n) = internal.ppn {
161                    result += n.size();
162                }
163                if let Some(ref n) = internal.pnp {
164                    result += n.size();
165                }
166                if let Some(ref n) = internal.pnn {
167                    result += n.size();
168                }
169                if let Some(ref n) = internal.npp {
170                    result += n.size();
171                }
172                if let Some(ref n) = internal.npn {
173                    result += n.size();
174                }
175                if let Some(ref n) = internal.nnp {
176                    result += n.size();
177                }
178                if let Some(ref n) = internal.nnn {
179                    result += n.size();
180                }
181                result
182            }
183        }
184    }
185}
186
187impl<P> OcNode<P>
188where
189    P: IsBuildable3D + Clone,
190{
191    /// Creates a new OcNode from a min and max position and the data it should hold
192    pub fn new(bb: &BoundingBox3D, pc: Vec<P>) -> Result<OcNode<P>> {
193        if pc.len() == 1 {
194            return Ok(OcNode::Leaf(pc[0].clone()));
195        };
196        let mut pcppp = Vec::new();
197        let mut pcppn = Vec::new();
198        let mut pcpnp = Vec::new();
199        let mut pcpnn = Vec::new();
200        let mut pcnpp = Vec::new();
201        let mut pcnpn = Vec::new();
202        let mut pcnnp = Vec::new();
203        let mut pcnnn = Vec::new();
204
205        let bbppp = Self::calc_sub_min_max(Direction::PPP, bb)?;
206        let bbppn = Self::calc_sub_min_max(Direction::PPN, bb)?;
207        let bbpnp = Self::calc_sub_min_max(Direction::PNP, bb)?;
208        let bbpnn = Self::calc_sub_min_max(Direction::PNN, bb)?;
209        let bbnpp = Self::calc_sub_min_max(Direction::NPP, bb)?;
210        let bbnpn = Self::calc_sub_min_max(Direction::NPN, bb)?;
211        let bbnnp = Self::calc_sub_min_max(Direction::NNP, bb)?;
212        let bbnnn = Self::calc_sub_min_max(Direction::NNN, bb)?;
213
214        for p in pc {
215            if bbppp.contains(&p) {
216                pcppp.push(p);
217            } else if bbppn.contains(&p) {
218                pcppn.push(p);
219            } else if bbpnp.contains(&p) {
220                pcpnp.push(p);
221            } else if bbpnn.contains(&p) {
222                pcpnn.push(p);
223            } else if bbnpp.contains(&p) {
224                pcnpp.push(p);
225            } else if bbnpn.contains(&p) {
226                pcnpn.push(p);
227            } else if bbnnp.contains(&p) {
228                pcnnp.push(p);
229            } else if bbnnn.contains(&p) {
230                pcnnn.push(p);
231            }
232        }
233
234        let ppp = Self::build_subnode(pcppp, &bbppp);
235        let ppn = Self::build_subnode(pcppn, &bbppn);
236        let pnp = Self::build_subnode(pcpnp, &bbpnp);
237        let pnn = Self::build_subnode(pcpnn, &bbpnn);
238        let npp = Self::build_subnode(pcnpp, &bbnpp);
239        let npn = Self::build_subnode(pcnpn, &bbnpn);
240        let nnp = Self::build_subnode(pcnnp, &bbnnp);
241        let nnn = Self::build_subnode(pcnnn, &bbnnn);
242
243        let result: Internal<P> = Internal {
244            ppp,
245            ppn,
246            pnp,
247            pnn,
248            npp,
249            npn,
250            nnp,
251            nnn,
252        };
253
254        Ok(OcNode::Node(result))
255    }
256    /// Calculates the min and max values of sub nodes of an OcTree
257    fn calc_sub_min_max(dir: Direction, bb: &BoundingBox3D) -> Result<BoundingBox3D> {
258        let (min, max) = (bb.min_p(), bb.max_p());
259        let middle = center_3d(&min, &max);
260
261        let px = max.x();
262        let py = max.y();
263        let pz = max.z();
264        let nx = min.x();
265        let ny = min.y();
266        let nz = min.z();
267        let mx = middle.x();
268        let my = middle.y();
269        let mz = middle.z();
270
271        match dir {
272            Direction::PPP => BoundingBox3D::new(&middle, &max),
273            Direction::PPN => {
274                BoundingBox3D::new(&Point3D::new(mx, my, nz), &Point3D::new(px, py, mz))
275            }
276            Direction::PNP => {
277                BoundingBox3D::new(&Point3D::new(mx, ny, mz), &Point3D::new(px, my, pz))
278            }
279            Direction::PNN => {
280                BoundingBox3D::new(&Point3D::new(mx, ny, nz), &Point3D::new(px, my, mz))
281            }
282            Direction::NPP => {
283                BoundingBox3D::new(&Point3D::new(nx, my, mz), &Point3D::new(mx, py, pz))
284            }
285            Direction::NPN => {
286                BoundingBox3D::new(&Point3D::new(nx, my, nz), &Point3D::new(mx, py, mz))
287            }
288            Direction::NNP => {
289                BoundingBox3D::new(&Point3D::new(nx, ny, mz), &Point3D::new(mx, my, pz))
290            }
291            Direction::NNN => BoundingBox3D::new(&min.clone(), &middle),
292        }
293    }
294    /// Creates a child node
295    fn build_subnode(pc: Vec<P>, bb: &BoundingBox3D) -> Option<Box<OcNode<P>>> {
296        match pc.len() {
297            0 => None,
298            _ => match OcNode::new(bb, pc) {
299                Err(_) => None,
300                Ok(x) => Some(Box::new(x)),
301            },
302        }
303    }
304}
305
306impl<P> OcNode<P>
307where
308    P: IsBuildable3D + Clone + Default,
309{
310    /// Collects all points within the node until a certain depth
311    pub fn collect(&self, depth: i8, maxdepth: i8, pc: &mut PointCloud3D<P>) {
312        let only_collect_centers = maxdepth >= 0 && depth > maxdepth;
313        match self {
314            &OcNode::Leaf(ref p) => pc.push(p.clone()),
315
316            &OcNode::Node(ref internal) => {
317                if let Some(ref n) = internal.ppp {
318                    Self::collect_center_or_all(n, only_collect_centers, depth, maxdepth, pc);
319                }
320                if let Some(ref n) = internal.ppn {
321                    Self::collect_center_or_all(n, only_collect_centers, depth, maxdepth, pc);
322                }
323                if let Some(ref n) = internal.pnp {
324                    Self::collect_center_or_all(n, only_collect_centers, depth, maxdepth, pc);
325                }
326                if let Some(ref n) = internal.pnn {
327                    Self::collect_center_or_all(n, only_collect_centers, depth, maxdepth, pc);
328                }
329                if let Some(ref n) = internal.npp {
330                    Self::collect_center_or_all(n, only_collect_centers, depth, maxdepth, pc);
331                }
332                if let Some(ref n) = internal.npn {
333                    Self::collect_center_or_all(n, only_collect_centers, depth, maxdepth, pc);
334                }
335                if let Some(ref n) = internal.nnp {
336                    Self::collect_center_or_all(n, only_collect_centers, depth, maxdepth, pc);
337                }
338                if let Some(ref n) = internal.nnn {
339                    Self::collect_center_or_all(n, only_collect_centers, depth, maxdepth, pc);
340                }
341            }
342        }
343    }
344    /// Depending on a flag either returns the child points or the center of gravity
345    fn collect_center_or_all(
346        n: &OcNode<P>,
347        only_collect_centers: bool,
348        depth: i8,
349        maxdepth: i8,
350        pc: &mut PointCloud3D<P>,
351    ) {
352        if only_collect_centers {
353            let mut sub_pc = PointCloud3D::new();
354            n.collect(depth + 1, maxdepth, &mut sub_pc);
355            if let Ok(c) = sub_pc.center_of_gravity() {
356                let mut p = P::default();
357                p.from(&c);
358                pc.push(p);
359            }
360        } else {
361            n.collect(depth + 1, maxdepth, pc);
362        }
363    }
364}
rust_3d/oc_tree.rs

rust_3d/
oc_tree.rs
