diff --git a/.DS_Store b/.DS_Store deleted file mode 100644 index 0b58850b..00000000 Binary files a/.DS_Store and /dev/null differ diff --git a/Cargo.lock b/Cargo.lock index 4ce65b65..f17834cd 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1353,6 +1353,11 @@ dependencies = [ "libc 0.2.54 (registry+https://github.com/rust-lang/crates.io-index)", ] +[[package]] +name = "objekt" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" + [[package]] name = "octree_web_viewer" version = "0.1.0" @@ -1535,6 +1540,7 @@ dependencies = [ "libc 0.2.54 (registry+https://github.com/rust-lang/crates.io-index)", "num 0.1.42 (registry+https://github.com/rust-lang/crates.io-index)", "num-traits 0.1.43 (registry+https://github.com/rust-lang/crates.io-index)", + "objekt 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)", "pbr 1.0.1 (registry+https://github.com/rust-lang/crates.io-index)", "point_viewer_proto_rust 0.1.0", "protobuf 2.0.5 (registry+https://github.com/rust-lang/crates.io-index)", @@ -3079,6 +3085,7 @@ dependencies = [ "checksum num-traits 0.2.4 (registry+https://github.com/rust-lang/crates.io-index)" = "775393e285254d2f5004596d69bb8bc1149754570dcc08cf30cabeba67955e28" "checksum num_cpus 0.2.13 (registry+https://github.com/rust-lang/crates.io-index)" = "cee7e88156f3f9e19bdd598f8d6c9db7bf4078f99f8381f43a55b09648d1a6e3" "checksum num_cpus 1.8.0 (registry+https://github.com/rust-lang/crates.io-index)" = "c51a3322e4bca9d212ad9a158a02abc6934d005490c054a2778df73a70aa0a30" +"checksum objekt 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)" = "2069a3ae3dad97a4ae47754e8f47e5d2f1fd32ab7ad8a84bb31d051faa59cc3c" "checksum openssl 0.9.24 (registry+https://github.com/rust-lang/crates.io-index)" = "a3605c298474a3aa69de92d21139fb5e2a81688d308262359d85cdd0d12a7985" "checksum openssl-sys 0.9.39 (registry+https://github.com/rust-lang/crates.io-index)" = "278c1ad40a89aa1e741a1eed089a2f60b18fab8089c3139b542140fc7d674106" "checksum owning_ref 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)" = "49a4b8ea2179e6a2e27411d3bca09ca6dd630821cf6894c6c7c8467a8ee7ef13" diff --git a/Cargo.toml b/Cargo.toml index 9b6a2396..fd1daa4d 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -26,6 +26,7 @@ error-chain = "0.11.0" fnv = "1.0.6" num = "0.1.36" num-traits = "0.1.36" +objekt = "0.1.2" pbr = "1.0.0-alpha.1" protobuf = "2.0.0" scoped-pool = "^0.1" diff --git a/octree_web_viewer/client/main.ts b/octree_web_viewer/client/main.ts index 4da34dfd..671c3347 100644 --- a/octree_web_viewer/client/main.ts +++ b/octree_web_viewer/client/main.ts @@ -45,7 +45,7 @@ class App { const result = window .fetch(request) - .then((response) => { return response.text(); }) // todo error handling? + .then((response) => { return response.text(); }) // TODO(catevita, negin-z): error handling? return result; } @@ -128,7 +128,7 @@ class App { } private cleanup() { - // TODO(negin-z) block requests from the viewer that is going to be replaced + // TODO(negin-z): block requests from the viewer that is going to be replaced this.removeControls(); if (this.renderer) { this.renderArea.removeChild(this.renderer.domElement); @@ -171,7 +171,7 @@ class App { .name('Point Cloud ID') .onFinishChange(this.run); - // TODO(negin-z) error handling + // TODO(negin-z): error handling this.fetchDefaultOctreeId() .then(this.setOctreeId) .then(this.run); @@ -180,7 +180,7 @@ class App { } private onWindowResize() { - // TODO(cvitadello) why is the window size used here? + // TODO(cvitadello): why is the window size used here? this.camera.aspect = window.innerWidth / window.innerHeight; this.camera.updateProjectionMatrix(); this.renderer.setSize(window.innerWidth, window.innerHeight); diff --git a/point_cloud_client/src/bin/test.rs b/point_cloud_client/src/bin/test.rs index dcab01b0..9afb025b 100644 --- a/point_cloud_client/src/bin/test.rs +++ b/point_cloud_client/src/bin/test.rs @@ -4,11 +4,14 @@ use cgmath::Point3; use collision::Aabb3; use point_cloud_client::PointCloudClient; -use point_viewer::errors::*; -use point_viewer::octree::{OctreeFactory, PointCulling, PointLocation}; +use point_viewer::errors::{ErrorKind, Result}; +use point_viewer::octree::{OctreeFactory, PointLocation, PointQuery}; use point_viewer::PointData; use structopt::StructOpt; +// size for batch +const BATCH_SIZE: usize = 1_000_000; + fn point3f64_from_str(s: &str) -> std::result::Result, &'static str> { let coords: std::result::Result, &'static str> = s .split(|c| c == ' ' || c == ',' || c == ';') @@ -54,17 +57,17 @@ fn main() { let num_points = args.num_points; let point_cloud_client = PointCloudClient::new(&args.locations, OctreeFactory::new()) .expect("Couldn't create octree client."); - let point_location = PointLocation { - culling: PointCulling::Aabb(Aabb3::new(args.min, args.max)), + let point_location = PointQuery { + location: PointLocation::Aabb(Aabb3::new(args.min, args.max)), global_from_local: None, }; let mut point_count: usize = 0; let mut print_count: usize = 1; let callback_func = |point_data: PointData| -> Result<()> { point_count += point_data.position.len(); - if point_count >= print_count * 1_000_000 { + if point_count >= print_count * BATCH_SIZE { print_count += 1; - println!("Streamed {}M points", point_count / 1_000_000); + println!("Streamed {}M points", point_count / BATCH_SIZE); } if point_count >= num_points { return Err(std::io::Error::new( diff --git a/point_cloud_client/src/lib.rs b/point_cloud_client/src/lib.rs index 662fe3de..8356ca4c 100644 --- a/point_cloud_client/src/lib.rs +++ b/point_cloud_client/src/lib.rs @@ -1,7 +1,7 @@ use collision::{Aabb, Aabb3, Union}; use point_viewer::errors::*; use point_viewer::octree::{ - BatchIterator, Octree, OctreeFactory, PointLocation, NUM_POINTS_PER_BATCH, + BatchIterator, Octree, OctreeFactory, PointQuery, NUM_POINTS_PER_BATCH, }; use point_viewer::PointData; @@ -39,7 +39,7 @@ impl PointCloudClient { &self.aabb } - pub fn for_each_point_data(&self, point_location: &PointLocation, mut func: F) -> Result<()> + pub fn for_each_point_data(&self, point_location: &PointQuery, mut func: F) -> Result<()> where F: FnMut(PointData) -> Result<()>, { diff --git a/point_viewer_grpc/src/bin/octree_benchmark.rs b/point_viewer_grpc/src/bin/octree_benchmark.rs index d5dbe011..a341d817 100644 --- a/point_viewer_grpc/src/bin/octree_benchmark.rs +++ b/point_viewer_grpc/src/bin/octree_benchmark.rs @@ -20,12 +20,15 @@ use std::path::{Path, PathBuf}; use std::str::FromStr; use std::sync::Arc; -use point_viewer::octree::{octree_from_directory, OctreeFactory}; -use point_viewer::Point; +use point_viewer::octree::{ + octree_from_directory, BatchIterator, OctreeFactory, PointLocation, PointQuery, +}; use point_viewer_grpc::proto_grpc::OctreeClient; use point_viewer_grpc::service::start_grpc_server; use point_viewer_grpc_proto_rust::proto; +// size for batch +const BATCH_SIZE: usize = 1_000_000; fn main() { let matches = clap::App::new("octree_benchmark") .args(&[ @@ -53,7 +56,7 @@ fn main() { .value_of("octree_directory") .expect("octree_directory not given"), ); - let num_points = u64::from_str(matches.value_of("num-points").unwrap_or("50000000")) + let num_points = usize::from_str(matches.value_of("num-points").unwrap_or("50000000")) .expect("num-points needs to be a number"); if matches.is_present("no-client") { server_benchmark(&octree_directory, num_points) @@ -63,26 +66,31 @@ fn main() { } } -fn server_benchmark(octree_directory: &Path, num_points: u64) { +fn server_benchmark(octree_directory: &Path, num_points: usize) { let octree = octree_from_directory(octree_directory).unwrap_or_else(|_| { panic!( "Could not create octree from '{}'", octree_directory.display() ) }); - let mut counter: u64 = 0; - octree.all_points().for_each(|_p: Point| { - if counter % 1_000_000 == 0 { - println!("Streamed {}M points", counter / 1_000_000); - } - counter += 1; - if counter == num_points { + let mut counter: usize = 0; + let all_points = PointQuery { + location: PointLocation::AllPoints(), + global_from_local: None, + }; + let mut batch_iterator = BatchIterator::new(&octree, &all_points, BATCH_SIZE); + + let _result = batch_iterator.try_for_each_batch(move |point_data| { + counter += point_data.position.len(); + if counter >= num_points { std::process::exit(0) } + println!("Streamed {}M points", counter / BATCH_SIZE); + Ok(()) }); } -fn full_benchmark(octree_directory: &Path, num_points: u64, port: u16) { +fn full_benchmark(octree_directory: &Path, num_points: usize, port: u16) { let octree_factory = OctreeFactory::new(); let mut server = start_grpc_server("0.0.0.0", port, octree_directory, octree_factory); server.start(); @@ -94,12 +102,12 @@ fn full_benchmark(octree_directory: &Path, num_points: u64, port: u16) { let req = proto::GetAllPointsRequest::new(); let receiver = client.get_all_points(&req).unwrap(); - let mut counter: u64 = 0; + let mut counter: usize = 0; 'outer: for rep in receiver.wait() { for _pos in rep.expect("Stream error").get_positions().iter() { - if counter % 1_000_000 == 0 { - println!("Streamed {}M points", counter / 1_000_000); + if counter % BATCH_SIZE == 0 { + println!("Streamed {}M points", counter / BATCH_SIZE); } counter += 1; if counter == num_points { diff --git a/point_viewer_grpc/src/service.rs b/point_viewer_grpc/src/service.rs index 853e6690..1c3c098d 100644 --- a/point_viewer_grpc/src/service.rs +++ b/point_viewer_grpc/src/service.rs @@ -14,6 +14,7 @@ use crate::proto; use crate::proto_grpc; +use crate::Color; use cgmath::{ Decomposed, Matrix4, PerspectiveFov, Point3, Quaternion, Rad, Transform, Vector2, Vector3, }; @@ -26,8 +27,8 @@ use grpcio::{ }; use point_viewer::errors::*; use point_viewer::math::{Isometry3, OrientedBeam}; -use point_viewer::octree::{NodeId, Octree, OctreeFactory}; -use point_viewer::Point; +use point_viewer::octree::{self, BatchIterator, NodeId, Octree, OctreeFactory, PointQuery}; +use point_viewer::{LayerData, PointData}; use protobuf::Message; use std::collections::HashMap; use std::path::PathBuf; @@ -46,14 +47,6 @@ struct OctreeService { factory: OctreeFactory, } -#[derive(Debug)] -enum OctreeQuery { - Frustum(Matrix4), - Box(Aabb3), - FullPointcloud, - OrientedBeam(Box), -} - fn send_fail_stream(ctx: &RpcContext, sink: ServerStreamingSink, err_str: String) { let f = sink .fail(RpcStatus::new(RpcStatusCode::Internal, Some(err_str))) @@ -145,12 +138,11 @@ impl proto_grpc::Octree for OctreeService { disp: translation, }; let frustum_matrix = projection_matrix.concat(&view_transform.into()); - self.stream_points_back_to_sink( - OctreeQuery::Frustum(frustum_matrix), - &req.octree_id, - &ctx, - resp, - ) + let point_location = PointQuery { + location: octree::PointLocation::Frustum(frustum_matrix), + global_from_local: None, + }; + self.stream_points_back_to_sink(point_location, &req.octree_id, &ctx, resp) } fn get_points_in_box( @@ -168,7 +160,11 @@ impl proto_grpc::Octree for OctreeService { Point3::new(max.x, max.y, max.z), ) }; - self.stream_points_back_to_sink(OctreeQuery::Box(bounding_box), &req.octree_id, &ctx, resp) + let point_location = PointQuery { + location: octree::PointLocation::Aabb(bounding_box), + global_from_local: None, + }; + self.stream_points_back_to_sink(point_location, &req.octree_id, &ctx, resp) } fn get_all_points( @@ -177,7 +173,11 @@ impl proto_grpc::Octree for OctreeService { req: proto::GetAllPointsRequest, resp: ServerStreamingSink, ) { - self.stream_points_back_to_sink(OctreeQuery::FullPointcloud, &req.octree_id, &ctx, resp) + let point_location = PointQuery { + location: octree::PointLocation::AllPoints(), + global_from_local: None, + }; + self.stream_points_back_to_sink(point_location, &req.octree_id, &ctx, resp) } fn get_points_in_oriented_beam( @@ -202,19 +202,18 @@ impl proto_grpc::Octree for OctreeService { }; let beam = OrientedBeam::new(Isometry3::new(rotation, translation), half_extent); - self.stream_points_back_to_sink( - OctreeQuery::OrientedBeam(Box::new(beam)), - &req.octree_id, - &ctx, - resp, - ) + let point_location = PointQuery { + location: octree::PointLocation::OrientedBeam(beam), + global_from_local: None, + }; + self.stream_points_back_to_sink(point_location, &req.octree_id, &ctx, resp) } } impl OctreeService { fn stream_points_back_to_sink( &self, - query: OctreeQuery, + query: PointQuery, octree_id: &str, ctx: &RpcContext, resp: ServerStreamingSink, @@ -266,57 +265,74 @@ impl OctreeService { // Proto message must be below 4 MB. let max_message_size = 4 * 1024 * 1024; - let mut reply_size = 0; + let num_points_per_batch: usize = max_message_size / bytes_per_point as usize; { // Extra scope to make sure that 'func' does not outlive 'reply'. - let func = |p: Point| { - { - let mut v = point_viewer::proto::Vector3d::new(); - v.set_x(p.position.x); - v.set_y(p.position.y); - v.set_z(p.position.z); - reply.mut_positions().push(v); - } - - { - let mut v = point_viewer::proto::Color::new(); - let clr = p.color.to_f32(); - v.set_red(clr.red); - v.set_green(clr.green); - v.set_blue(clr.blue); - v.set_alpha(clr.alpha); - reply.mut_colors().push(v); - } - - if let Some(i) = p.intensity { - reply.mut_intensities().push(i); - } - - reply_size += bytes_per_point; - if reply_size > max_message_size - bytes_per_point { - tx.send((reply.clone(), WriteFlags::default())).unwrap(); - reply.mut_positions().clear(); - reply.mut_colors().clear(); - reply.mut_intensities().clear(); - reply_size = 0; - } - }; - match query { - OctreeQuery::Box(bounding_box) => service_data - .octree - .points_in_box(&bounding_box) - .for_each(func), - OctreeQuery::Frustum(frustum_matrix) => service_data - .octree - .points_in_frustum(&frustum_matrix) - .for_each(func), - OctreeQuery::FullPointcloud => service_data.octree.all_points().for_each(func), - OctreeQuery::OrientedBeam(beam) => service_data - .octree - .points_in_oriented_beam(&beam) - .for_each(func), + // this function is currently not efficiently implemented + let func = |p_data: PointData| { + reply.positions = p_data + .position + .iter() + .map(|p| { + let mut v = point_viewer::proto::Vector3d::new(); + v.set_x(p.x); + v.set_y(p.y); + v.set_z(p.z); + v + }) + .collect(); + + reply.colors = match p_data.layers.get(&"color".to_string()) { + Some(LayerData::U8Vec4(data)) => data + .iter() + .map(|p| { + let rgb8: Color = crate::Color { + red: p.x, + green: p.y, + blue: p.z, + alpha: p.w, + }; + let rgb32: Color = crate::Color::to_f32(rgb8); + let mut v = point_viewer::proto::Color::new(); + v.set_red(rgb32.red); + v.set_green(rgb32.green); + v.set_blue(rgb32.blue); + v.set_alpha(rgb32.alpha); + v + }) + .collect(), + _ => { + return Err(std::io::Error::new( + std::io::ErrorKind::InvalidData, + "Color format is not u8", + ) + .into()); + } + }; + + reply.intensities = match p_data.layers.get(&"intensity".to_string()) { + Some(LayerData::F32(data)) => data.clone(), + _ => { + return Err(std::io::Error::new( + std::io::ErrorKind::InvalidData, + "Intensity format is not f32", + ) + .into()); + } + }; + + tx.send((reply.clone(), WriteFlags::default())).unwrap(); + reply.mut_positions().clear(); + reply.mut_colors().clear(); + reply.mut_intensities().clear(); + Ok(()) }; + + let mut batch_iterator = + BatchIterator::new(&service_data.octree, &query, num_points_per_batch); + // TODO(catevita): missing error handling for the thread + let _result = batch_iterator.try_for_each_batch(func); } tx.send((reply, WriteFlags::default())).unwrap(); }); diff --git a/sdl_viewer/src/bin/sdl_viewer.rs b/sdl_viewer/src/bin/sdl_viewer.rs index a5992b75..7e92715b 100644 --- a/sdl_viewer/src/bin/sdl_viewer.rs +++ b/sdl_viewer/src/bin/sdl_viewer.rs @@ -41,6 +41,6 @@ impl Extension for NullExtension { fn main() { let octree_factory = OctreeFactory::new().register("grpc://", octree_from_grpc_address); - // TODO (catevita) hide octree factory details, simplify the run method interface + // TODO(catevita): hide octree factory details, simplify the run method interface run::(octree_factory); } diff --git a/src/math.rs b/src/math.rs index 11b0c6e5..eadfa461 100644 --- a/src/math.rs +++ b/src/math.rs @@ -13,14 +13,61 @@ // limitations under the License. use cgmath::{ - BaseFloat, Decomposed, EuclideanSpace, InnerSpace, Point3, Quaternion, Rotation, Vector2, - Vector3, + BaseFloat, Decomposed, EuclideanSpace, InnerSpace, Matrix4, Point3, Quaternion, Rotation, + Vector2, Vector3, Vector4, }; -use collision::{Aabb, Aabb3}; +use collision::{Aabb, Aabb3, Contains, Relation}; use num_traits::identities::One; use num_traits::Float; +use std::fmt::Debug; use std::ops::Mul; +pub trait PointCulling: objekt::Clone + Debug + Sync + Send +where + S: BaseFloat + Sync + Send, +{ + fn contains(&self, point: &Point3) -> bool; + // TODO(catevita): return Relation + fn intersects(&self, aabb: &Aabb3) -> bool; + fn transform(&self, isometry: &Isometry3) -> Box>; +} + +impl PointCulling for Aabb3 +where + S: 'static + BaseFloat + Sync + Send, +{ + fn intersects(&self, aabb: &Aabb3) -> bool { + let separating_axes = &[Vector3::unit_x(), Vector3::unit_y(), Vector3::unit_z()]; + intersects(&self.to_corners(), separating_axes, aabb) + } + + fn contains(&self, p: &Point3) -> bool { + Contains::contains(self, p) + } + + fn transform(&self, isometry: &Isometry3) -> Box> { + Obb::from(*self).transform(isometry) + } +} + +/// Implementation of PointCulling to return all points +#[derive(Clone, Debug)] +pub struct AllPoints {} + +impl PointCulling for AllPoints +where + S: BaseFloat + Sync + Send, +{ + fn intersects(&self, _aabb: &Aabb3) -> bool { + true + } + fn contains(&self, _p: &Point3) -> bool { + true + } + fn transform(&self, _isometry: &Isometry3) -> Box> { + Box::new(AllPoints {}) + } +} #[derive(Debug, Clone)] pub struct Cube { min: Point3, @@ -190,6 +237,15 @@ pub struct Obb { separating_axes: Vec>, } +impl From<&Aabb3> for Obb { + fn from(aabb: &Aabb3) -> Self { + Obb::new( + Isometry3::new(Quaternion::one(), EuclideanSpace::to_vec(aabb.center())), + aabb.dim() / (S::one() + S::one()), + ) + } +} + impl From> for Obb { fn from(aabb: Aabb3) -> Self { Obb::new( @@ -209,26 +265,9 @@ impl Obb { } } - pub fn intersects(&self, aabb: &Aabb3) -> bool { - intersects(&self.corners, &self.separating_axes, aabb) - } - - pub fn contains(&self, p: &Point3) -> bool { - let Point3 { x, y, z } = - self.isometry_inv.rotation.rotate_point(*p) + self.isometry_inv.translation; - x.abs() <= self.half_extent.x - && y.abs() <= self.half_extent.y - && z.abs() <= self.half_extent.z - } - - pub fn transform(&self, isometry: &Isometry3) -> Self { - Self::new(isometry * &self.isometry_inv.inverse(), self.half_extent) - } - fn precompute_corners(isometry: &Isometry3, half_extent: &Vector3) -> [Point3; 8] { - let corner_from = |x: S, y: S, z: S| { - isometry.rotation.rotate_point(Point3::new(x, y, z)) + isometry.translation - }; + let corner_from = + |x, y, z| isometry.rotation.rotate_point(Point3::new(x, y, z)) + isometry.translation; [ corner_from(-half_extent.x, -half_extent.y, -half_extent.z), corner_from(half_extent.x, -half_extent.y, -half_extent.z), @@ -276,19 +315,43 @@ impl Obb { } } +impl PointCulling for Obb +where + S: 'static + BaseFloat + Sync + Send, +{ + fn intersects(&self, aabb: &Aabb3) -> bool { + intersects(&self.corners, &self.separating_axes, aabb) + } + + fn contains(&self, p: &Point3) -> bool { + let Point3 { x, y, z } = + self.isometry_inv.rotation.rotate_point(*p) + self.isometry_inv.translation; + x.abs() <= self.half_extent.x + && y.abs() <= self.half_extent.y + && z.abs() <= self.half_extent.z + } + + fn transform(&self, isometry: &Isometry3) -> Box> { + Box::new(Self::new( + isometry * &self.isometry_inv.inverse(), + self.half_extent, + )) + } +} + #[derive(Debug, Clone)] -pub struct OrientedBeam { +pub struct OrientedBeam { // The members here are an implementation detail and differ from the // minimal representation in the gRPC message to speed up operations. // Isometry_inv is the transform from world coordinates into "beam coordinates". - isometry_inv: Isometry3, - half_extent: Vector2, - corners: [Point3; 4], - separating_axes: Vec>, + isometry_inv: Isometry3, + half_extent: Vector2, + corners: [Point3; 4], + separating_axes: Vec>, } -impl OrientedBeam { - pub fn new(isometry: Isometry3, half_extent: Vector2) -> Self { +impl OrientedBeam { + pub fn new(isometry: Isometry3, half_extent: Vector2) -> Self { OrientedBeam { isometry_inv: isometry.inverse(), half_extent, @@ -297,27 +360,9 @@ impl OrientedBeam { } } - pub fn intersects(&self, aabb: &Aabb3) -> bool { - intersects(&self.corners, &self.separating_axes, aabb) - } - - pub fn contains(&self, p: &Point3) -> bool { - // What is the point in beam coordinates? - let Point3 { x, y, .. } = - self.isometry_inv.rotation.rotate_point(*p) + self.isometry_inv.translation; - x.abs() <= self.half_extent.x && y.abs() <= self.half_extent.y - } - - pub fn transform(&self, isometry: &Isometry3) -> Self { - Self::new(isometry * &self.isometry_inv.inverse(), self.half_extent) - } - - fn precompute_corners( - isometry: &Isometry3, - half_extent: &Vector2, - ) -> [Point3; 4] { - let corner_from = |x: f64, y: f64| { - isometry.rotation.rotate_point(Point3::new(x, y, 0.0)) + isometry.translation + fn precompute_corners(isometry: &Isometry3, half_extent: &Vector2) -> [Point3; 4] { + let corner_from = |x, y| { + isometry.rotation.rotate_point(Point3::new(x, y, S::zero())) + isometry.translation }; [ corner_from(half_extent.x, half_extent.y), @@ -331,7 +376,7 @@ impl OrientedBeam { // Currently we have a beam which is infinite in both directions. // If we defined a beam on one side of the earth pointing towards the sky, // it will also collect points on the other side of the earth, which is undesired. - fn precompute_separating_axes(rotation: &Quaternion) -> Vec> { + fn precompute_separating_axes(rotation: &Quaternion) -> Vec> { // The separating axis needs to be perpendicular to the beam's main // axis, i.e. the possible axes are the cross product of the three unit // vectors with the beam's main axis and the beam's face normals. @@ -349,6 +394,29 @@ impl OrientedBeam { } } +impl PointCulling for OrientedBeam +where + S: 'static + BaseFloat + Sync + Send, +{ + fn intersects(&self, aabb: &Aabb3) -> bool { + intersects(&self.corners, &self.separating_axes, aabb) + } + + fn contains(&self, p: &Point3) -> bool { + // What is the point in beam coordinates? + let Point3 { x, y, .. } = + self.isometry_inv.rotation.rotate_point(*p) + self.isometry_inv.translation; + x.abs() <= self.half_extent.x && y.abs() <= self.half_extent.y + } + + fn transform(&self, isometry: &Isometry3) -> Box> { + Box::new(Self::new( + isometry * &self.isometry_inv.inverse(), + self.half_extent, + )) + } +} + #[cfg(test)] mod tests { use super::*; @@ -359,12 +427,12 @@ mod tests { // bpy.data.objects['Beam'].rotation_euler.to_quaternion() and // bpy.data.objects['Beam'].location. - fn some_beam() -> OrientedBeam { + fn some_beam() -> OrientedBeam { let quater = Quaternion::new( - 0.9292423725128174, - -0.2677415907382965, - -0.20863021910190582, - -0.14593297243118286, + 0.929_242_372_512_817_4, + -0.267_741_590_738_296_5, + -0.208_630_219_101_905_82, + -0.145_932_972_431_182_86, ); let translation = Vector3::new(1.0, 2.0, 0.0); let half_extent = Vector2::new(1.0, 1.0); @@ -428,3 +496,49 @@ mod tests { assert_eq!(arbitrary_obb.separating_axes.len(), 15); } } + +#[derive(Debug, Clone)] +pub struct Frustum { + matrix: Matrix4, + frustum: collision::Frustum, +} + +impl Frustum { + pub fn new(matrix: Matrix4) -> Self { + Frustum { + matrix, + frustum: collision::Frustum::from_matrix4(matrix).unwrap(), + } + } +} + +impl PointCulling for Frustum +where + S: 'static + BaseFloat + Sync + Send, +{ + fn contains(&self, point: &Point3) -> bool { + // TODO(ksavinash9) update after https://github.com/rustgd/collision-rs/issues/101 is resolved. + let v = Vector4::new(point.x, point.y, point.z, S::one()); + let clip_v = self.matrix * v; + clip_v.x.abs() < clip_v.w + && clip_v.y.abs() < clip_v.w + && S::zero() < clip_v.z + && clip_v.z < clip_v.w + } + + fn intersects(&self, aabb: &Aabb3) -> bool { + match self.frustum.contains(aabb) { + Relation::Cross => true, + Relation::In => true, + Relation::Out => false, + } + } + + fn transform(&self, isometry: &Isometry3) -> Box> { + let isometry = isometry.clone(); + let matrix: Matrix4 = Matrix4::from( + Into::, Quaternion>>::into(isometry), + ) * self.matrix; + Box::new(Frustum::new(matrix)) + } +} diff --git a/src/octree/batch_iterator.rs b/src/octree/batch_iterator.rs index faba7d65..2c996128 100644 --- a/src/octree/batch_iterator.rs +++ b/src/octree/batch_iterator.rs @@ -1,31 +1,47 @@ use crate::errors::*; -use crate::math::{Isometry3, Obb, OrientedBeam}; +use crate::math::PointCulling; +use crate::math::{AllPoints, Isometry3, Obb, OrientedBeam}; use crate::octree::{self, Octree}; use crate::{LayerData, Point, PointData}; -use cgmath::{Decomposed, Matrix4, Vector3, Vector4}; +use cgmath::{Matrix4, Vector3, Vector4}; use collision::Aabb3; use fnv::FnvHashMap; /// size for batch pub const NUM_POINTS_PER_BATCH: usize = 500_000; -///possible kind of iterators that can be evaluated in batch of points in BatchIterator #[allow(clippy::large_enum_variant)] -#[derive(Clone)] -pub enum PointCulling { - Any(), +#[derive(Debug, Clone)] +pub enum PointLocation { + AllPoints(), Aabb(Aabb3), - Obb(Obb), Frustum(Matrix4), - OrientedBeam(OrientedBeam), + Obb(Obb), + OrientedBeam(OrientedBeam), } -pub struct PointLocation { - pub culling: PointCulling, +#[derive(Clone, Debug)] +pub struct PointQuery { + pub location: PointLocation, // If set, culling and the returned points are interpreted to be in local coordinates. pub global_from_local: Option>, } +impl PointQuery { + pub fn get_point_culling(&self) -> Box> { + let culling: Box> = match &self.location { + PointLocation::AllPoints() => return Box::new(AllPoints {}), + PointLocation::Aabb(aabb) => Box::new(*aabb), + PointLocation::Frustum(matrix) => Box::new(octree::Frustum::new(*matrix)), + PointLocation::Obb(obb) => Box::new(obb.clone()), + PointLocation::OrientedBeam(beam) => Box::new(beam.clone()), + }; + match &self.global_from_local { + Some(global_from_local) => culling.transform(&global_from_local), + None => culling, + } + } +} /// current implementation of the stream of points used in BatchIterator struct PointStream<'a, F> where @@ -110,38 +126,19 @@ where /// Iterator on point batches pub struct BatchIterator<'a> { octree: &'a Octree, - culling: PointCulling, - local_from_global: Option>, + point_location: &'a PointQuery, batch_size: usize, } impl<'a> BatchIterator<'a> { - pub fn new(octree: &'a octree::Octree, location: &'a PointLocation, batch_size: usize) -> Self { - let culling = match &location.global_from_local { - Some(global_from_local) => match &location.culling { - PointCulling::Any() => PointCulling::Any(), - PointCulling::Aabb(aabb) => { - PointCulling::Obb(Obb::from(*aabb).transform(global_from_local)) - } - PointCulling::Obb(obb) => PointCulling::Obb(obb.transform(global_from_local)), - PointCulling::Frustum(frustum) => PointCulling::Frustum( - Matrix4::from(Decomposed { - scale: 1.0, - rot: global_from_local.rotation, - disp: global_from_local.translation, - }) * frustum, - ), - PointCulling::OrientedBeam(beam) => { - PointCulling::OrientedBeam(beam.transform(global_from_local)) - } - }, - None => location.culling.clone(), - }; - let local_from_global = location.global_from_local.as_ref().map(Isometry3::inverse); + pub fn new( + octree: &'a octree::Octree, + point_location: &'a PointQuery, + batch_size: usize, + ) -> Self { BatchIterator { octree, - culling, - local_from_global, + point_location, batch_size, } } @@ -151,16 +148,25 @@ impl<'a> BatchIterator<'a> { where F: FnMut(PointData) -> Result<()>, { - let mut point_stream = - PointStream::new(self.batch_size, self.local_from_global.clone(), &mut func); - let mut iterator: Box> = match &self.culling { - PointCulling::Any() => Box::new(self.octree.all_points()), - PointCulling::Aabb(aabb) => Box::new(self.octree.points_in_box(aabb)), - PointCulling::Obb(obb) => Box::new(self.octree.points_in_obb(obb)), - PointCulling::Frustum(frustum) => Box::new(self.octree.points_in_frustum(frustum)), - PointCulling::OrientedBeam(beam) => Box::new(self.octree.points_in_oriented_beam(beam)), - }; - iterator.try_for_each(|point: Point| point_stream.push_point_and_callback(point))?; - point_stream.callback() + //TODO(catevita): mutable function parallelization + let local_from_global = self + .point_location + .global_from_local + .clone() + .map(|t| t.inverse()); + let mut point_stream = PointStream::new(self.batch_size, local_from_global, &mut func); + // nodes iterator: retrieve nodes + let node_id_iterator = self.octree.nodes_in_location(self.point_location); + // operate on nodes + for node_id in node_id_iterator { + let point_iterator = self.octree.points_in_node(self.point_location, node_id); + for point in point_iterator { + point_stream.push_point_and_callback(point)?; + } + } + // TODO(catevita): return point data through mpsc channel + // TODO(catevita): apply mut function to received data + + Ok(()) } } diff --git a/src/octree/mod.rs b/src/octree/mod.rs index 13763793..d62f3959 100644 --- a/src/octree/mod.rs +++ b/src/octree/mod.rs @@ -13,11 +13,11 @@ // limitations under the License. use crate::errors::*; -use crate::math::{Cube, Obb, OrientedBeam}; +use crate::math::{Cube, Frustum}; use crate::proto; use crate::Point; use cgmath::{EuclideanSpace, Matrix4, Point3}; -use collision::{Aabb, Aabb3, Contains, Discrete, Frustum, Relation}; +use collision::{Aabb, Aabb3, Relation}; use fnv::FnvHashMap; use num::clamp; use std::cmp::Ordering; @@ -42,12 +42,10 @@ mod factory; pub use self::factory::OctreeFactory; mod octree_iterator; -pub use self::octree_iterator::{ - contains, intersecting_node_ids, AllPointsIterator, FilteredPointsIterator, NodeIdsIterator, -}; +pub use self::octree_iterator::{FilteredPointsIterator, NodeIdsIterator}; mod batch_iterator; -pub use self::batch_iterator::{BatchIterator, PointCulling, PointLocation, NUM_POINTS_PER_BATCH}; +pub use self::batch_iterator::{BatchIterator, PointLocation, PointQuery, NUM_POINTS_PER_BATCH}; #[cfg(test)] mod octree_test; @@ -225,7 +223,7 @@ impl Octree { } pub fn get_visible_nodes(&self, projection_matrix: &Matrix4) -> Vec { - let frustum = Frustum::from_matrix4(*projection_matrix).unwrap(); + let frustum = collision::Frustum::from_matrix4(*projection_matrix).unwrap(); let mut open = BinaryHeap::new(); maybe_push_node( &mut open, @@ -302,43 +300,27 @@ impl Octree { }) } - /// Returns the ids of all nodes that cut or are fully contained in 'aabb'. - pub fn points_in_box<'a>(&'a self, bbox: &'a Aabb3) -> FilteredPointsIterator<'a> { - let intersects = |aabb: &Aabb3| bbox.intersects(aabb); - let node_ids = intersecting_node_ids(self, &intersects); - let filter_func = Box::new(move |p: &Point| bbox.contains(&Point3::from_vec(p.position))); - FilteredPointsIterator::new(&self, node_ids, filter_func) - } - - pub fn points_in_obb<'a>(&'a self, obb: &'a Obb) -> FilteredPointsIterator<'a> { - let intersects = |aabb: &Aabb3| obb.intersects(aabb); - let node_ids = intersecting_node_ids(self, &intersects); - let filter_func = Box::new(move |p: &Point| obb.contains(&Point3::from_vec(p.position))); - FilteredPointsIterator::new(&self, node_ids, filter_func) - } - - pub fn points_in_oriented_beam<'a>( + pub fn nodes_in_location<'a>( &'a self, - beam: &'a OrientedBeam, - ) -> FilteredPointsIterator<'a> { - let intersects = |aabb: &Aabb3| beam.intersects(aabb); - let node_ids = intersecting_node_ids(self, &intersects); - let filter_func = Box::new(move |p: &Point| beam.contains(&Point3::from_vec(p.position))); - FilteredPointsIterator::new(&self, node_ids, filter_func) + location: &PointQuery, + ) -> NodeIdsIterator<'a, impl Fn(&NodeId, &'a Octree) -> bool> { + let container = location.get_point_culling(); + let filter_func = move |node_id: &NodeId, octree: &Octree| -> bool { + let current = &octree.nodes[&node_id]; + container.intersects(¤t.bounding_cube.to_aabb3()) + }; + NodeIdsIterator::new(&self, filter_func) } - pub fn points_in_frustum<'a>( + /// Returns the ids of all nodes that cut or are fully contained in 'aabb'. + pub fn points_in_node<'a>( &'a self, - frustum_matrix: &'a Matrix4, - ) -> FilteredPointsIterator<'a> { - let node_ids = self.get_visible_nodes(frustum_matrix); - let filter_func = - Box::new(move |p: &Point| contains(frustum_matrix, &Point3::from_vec(p.position))); - FilteredPointsIterator::new(&self, node_ids.into(), filter_func) - } - - pub fn all_points(&self) -> AllPointsIterator { - AllPointsIterator::new(&self) + location: &PointQuery, + node_id: NodeId, + ) -> FilteredPointsIterator bool> { + let container = location.get_point_culling(); + let filter_func = move |p: &Point| container.contains(&Point3::from_vec(p.position)); + FilteredPointsIterator::new(&self, node_id, filter_func) } /// return the bounding box saved in meta @@ -346,6 +328,7 @@ impl Octree { &self.meta.bounding_box } } + struct OpenNode { node: Node, relation: Relation, diff --git a/src/octree/octree_iterator.rs b/src/octree/octree_iterator.rs index 0c6f3b6f..f810cfd0 100644 --- a/src/octree/octree_iterator.rs +++ b/src/octree/octree_iterator.rs @@ -1,9 +1,5 @@ -use crate::math::Cube; - -use crate::octree::{ChildIndex, Node, NodeId, NodeIterator, Octree}; +use crate::octree::{ChildIndex, NodeId, NodeIterator, Octree}; use crate::Point; -use cgmath::{Matrix4, Point3, Vector4}; -use collision::Aabb3; use std::collections::VecDeque; /// returns an Iterator over the points of the current node @@ -18,123 +14,41 @@ fn get_node_iterator(octree: &Octree, node_id: &NodeId) -> NodeIterator { .expect("Could not read node points") } -/// iterator over the points of the octree that satisfy the condition expressed by a boolean function -pub struct FilteredPointsIterator<'a> { - octree: &'a Octree, - filter_func: Box bool + 'a>, - node_ids: VecDeque, +/// iterator over the points of a octree node that satisfy the condition expressed by a boolean function +pub struct FilteredPointsIterator { + filter_func: F, node_iterator: NodeIterator, } -impl<'a> FilteredPointsIterator<'a> { - pub fn new( - octree: &'a Octree, - node_ids: VecDeque, - filter_func: Box bool + 'a>, - ) -> Self { +impl FilteredPointsIterator +where + F: Fn(&Point) -> bool, +{ + pub fn new(octree: &Octree, node_id: NodeId, filter_func: F) -> FilteredPointsIterator { FilteredPointsIterator { - octree, filter_func, - node_ids, - node_iterator: NodeIterator::Empty, - } - } -} - -impl<'a> Iterator for FilteredPointsIterator<'a> { - type Item = Point; - - fn next(&mut self) -> Option { - loop { - while let Some(point) = self.node_iterator.next() { - if (self.filter_func)(&point) { - return Some(point); - } - } - self.node_iterator = match self.node_ids.pop_front() { - Some(node_id) => get_node_iterator(self.octree, &node_id), - None => return None, - }; - } - } -} -///iterator for all points in an octree -pub struct AllPointsIterator<'a> { - octree: &'a Octree, - node_iterator: NodeIterator, - open_list: VecDeque, -} - -impl<'a> AllPointsIterator<'a> { - pub fn new(octree: &'a Octree) -> Self { - AllPointsIterator { - octree, - node_iterator: NodeIterator::Empty, - open_list: vec![NodeId::from_level_index(0, 0)].into(), + node_iterator: get_node_iterator(octree, &node_id), } } } -impl<'a> Iterator for AllPointsIterator<'a> { +impl Iterator for FilteredPointsIterator +where + F: Fn(&Point) -> bool, +{ type Item = Point; fn next(&mut self) -> Option { - loop { - if let Some(point) = self.node_iterator.next() { + while let Some(point) = self.node_iterator.next() { + if (self.filter_func)(&point) { return Some(point); } - match self.open_list.pop_front() { - Some(current) => { - for child_index in 0..8 { - let child_id = current.get_child_id(ChildIndex::from_u8(child_index)); - if self.octree.nodes.contains_key(&child_id) { - self.open_list.push_back(child_id); - } - } - self.node_iterator = get_node_iterator(self.octree, ¤t); - } - None => return None, - } - } - } -} - -// TODO(ksavinash9) update after https://github.com/rustgd/collision-rs/issues/101 is resolved. -pub fn contains(projection_matrix: &Matrix4, point: &Point3) -> bool { - let v = Vector4::new(point.x, point.y, point.z, 1.); - let clip_v = projection_matrix * v; - clip_v.x.abs() < clip_v.w && clip_v.y.abs() < clip_v.w && 0. < clip_v.z && clip_v.z < clip_v.w -} - -pub fn intersecting_node_ids( - octree: &Octree, - intersects: &Fn(&Aabb3) -> bool, -) -> VecDeque { - let mut node_ids = VecDeque::new(); - let mut open_list: VecDeque = vec![Node::root_with_bounding_cube(Cube::bounding( - &octree.meta.bounding_box, - ))] - .into(); - while !open_list.is_empty() { - let current = open_list.pop_front().unwrap(); - if !intersects(¤t.bounding_cube.to_aabb3()) { - continue; - } - node_ids.push_back(current.id); - for child_index in 0..8 { - let child = current.get_child(ChildIndex::from_u8(child_index)); - if octree.nodes.contains_key(&child.id) { - open_list.push_back(child); - } } + None } - node_ids } -pub struct NodeIdsIterator<'a, F> -where - F: Fn(&NodeId, &Octree) -> bool + 'a, -{ +pub struct NodeIdsIterator<'a, F> { octree: &'a Octree, filter_func: F, node_ids: VecDeque, @@ -142,9 +56,9 @@ where impl<'a, F> NodeIdsIterator<'a, F> where - F: Fn(&NodeId, &Octree) -> bool + 'a, + F: Fn(&NodeId, &Octree) -> bool, { - pub fn new(octree: &'a Octree, filter_func: F) -> Self { + pub fn new(octree: &'a Octree, filter_func: F) -> NodeIdsIterator<'a, F> { NodeIdsIterator { octree, node_ids: vec![NodeId::from_level_index(0, 0)].into(), @@ -155,7 +69,7 @@ where impl<'a, F> Iterator for NodeIdsIterator<'a, F> where - F: Fn(&NodeId, &Octree) -> bool + 'a, + F: Fn(&NodeId, &'a Octree) -> bool, { type Item = NodeId; diff --git a/src/octree/octree_test.rs b/src/octree/octree_test.rs index 6e3d181f..41b0ef04 100644 --- a/src/octree/octree_test.rs +++ b/src/octree/octree_test.rs @@ -3,7 +3,7 @@ mod tests { use crate::color::Color; use crate::errors::Result; use crate::generation::build_octree; - use crate::octree::{self, BatchIterator, PointCulling, PointLocation}; + use crate::octree::{self, BatchIterator, PointLocation, PointQuery}; use crate::{Point, PointData}; use cgmath::{EuclideanSpace, Point3, Vector3}; use collision::{Aabb, Aabb3}; @@ -85,8 +85,8 @@ mod tests { // octree and iterator let octree = build_test_octree(); - let location = PointLocation { - culling: PointCulling::Any(), + let location = PointQuery { + location: PointLocation::AllPoints(), global_from_local: None, }; let mut batch_iterator = BatchIterator::new(&octree, &location, batch_size); @@ -128,8 +128,8 @@ mod tests { // octree and iterator let octree = build_big_test_octree(); - let location = PointLocation { - culling: PointCulling::Any(), + let location = PointQuery { + location: PointLocation::AllPoints(), global_from_local: None, }; let mut batch_iterator = BatchIterator::new(&octree, &location, batch_size); diff --git a/src/octree/on_disk.rs b/src/octree/on_disk.rs index d3602997..e9e1ce98 100644 --- a/src/octree/on_disk.rs +++ b/src/octree/on_disk.rs @@ -67,7 +67,7 @@ impl OctreeDataProvider for OnDiskOctreeDataProvider { } } -// TODO (catevita) refactor function for octree factory +// TODO(catevita): refactor function for octree factory pub fn octree_from_directory(directory: impl Into) -> Result> { let data_provider = OnDiskOctreeDataProvider { directory: directory.into(), diff --git a/xray/src/generation.rs b/xray/src/generation.rs index 06299ddc..7ff5ba9a 100644 --- a/xray/src/generation.rs +++ b/xray/src/generation.rs @@ -9,7 +9,7 @@ use fnv::{FnvHashMap, FnvHashSet}; use image::{self, GenericImage}; use num::clamp; use point_cloud_client::PointCloudClient; -use point_viewer::octree::{PointCulling, PointLocation}; +use point_viewer::octree::{PointLocation, PointQuery}; use point_viewer::{color::Color, math::Isometry3, LayerData, PointData}; use protobuf::Message; use quadtree::{ChildIndex, Node, NodeId, Rect}; @@ -463,8 +463,8 @@ pub fn xray_from_points( tile_background_color: Color, ) -> bool { let mut seen_any_points = false; - let point_location = PointLocation { - culling: PointCulling::Aabb(*bbox), + let point_location = PointQuery { + location: PointLocation::Aabb(*bbox), global_from_local: global_from_local.clone(), }; let _ = point_cloud_client.for_each_point_data(&point_location, |point_data| {