From b12927391411586372cbbedcabcd508330d34055 Mon Sep 17 00:00:00 2001
From: Brandon Thomas <echelon@gmail.com>
Date: Thu, 4 Feb 2021 04:44:01 -0500
Subject: [PATCH] left and right cull

---
 src/main_zeromq.rs                      | 15 +++++++++++++--
 src/zeromq/cpu_calculate_point_cloud.rs | 16 ++++++++++++++++
 2 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/src/main_zeromq.rs b/src/main_zeromq.rs
index 93e91165d2..ba3edff221 100644
--- a/src/main_zeromq.rs
+++ b/src/main_zeromq.rs
@@ -46,9 +46,20 @@ pub struct CommandArgs {
   pub wide: bool,
 
   /// Set the depth culling point
+  /// A good value for my office with a NFOV camera approximately located
+  /// parallel to where my desktop computer is located would be "3000"
   #[clap(long, default_value = "0")]
   pub depth_cull: i32,
 
+  /// Cull everything to the right of this point
+  /// "Right" is when the camera is inverted
+  /// A value of "500" is reasonable
+  #[clap(long, default_value = "0")]
+  pub right_cull: i32,
+
+  #[clap(long, default_value = "0")]
+  pub left_cull: i32,
+
   /// X offset.
   #[clap(long, default_value = "0", allow_hyphen_values = true)]
   pub xoff: i32,
@@ -142,7 +153,7 @@ fn main() -> AnyhowResult<()> {
     points = match maybe_points {
       Err(e) => {
         println!("Error: {:?}", e);
-        thread::sleep(Duration::from_millis(1000));
+        thread::sleep(Duration::from_millis(100));
         continue;
       },
       Ok(points) => points,
@@ -166,7 +177,7 @@ fn main() -> AnyhowResult<()> {
       },
       MessagingState::Sending_PointDataBegin => {
         if frame_number == 1 || frame_number % 10 == 0 {
-          println!("Sending frame number: {}", frame_number);
+          println!("Sending frame number: {} (points: {})", frame_number, points.len());
         }
 
         let message = encode_point_data(&mut points, true);
diff --git a/src/zeromq/cpu_calculate_point_cloud.rs b/src/zeromq/cpu_calculate_point_cloud.rs
index 4f93fec59b..fb4ab75e0c 100644
--- a/src/zeromq/cpu_calculate_point_cloud.rs
+++ b/src/zeromq/cpu_calculate_point_cloud.rs
@@ -142,6 +142,22 @@ pub fn calculate_point_cloud3(
 
                 let color = (*color_data.offset(i)) as ColorCameraPoint;
 
+                // The z-direction moves directly towards and away from the camera
+                if command_args.depth_cull != 0
+                    && z > command_args.depth_cull as f32 {
+                    continue;
+                }
+
+                if command_args.left_cull != 0
+                    && x > command_args.left_cull as f32 {
+                    continue;
+                }
+
+                if command_args.right_cull != 0
+                    && x < command_args.right_cull as f32 {
+                    continue;
+                }
+
                 if command_args.xoff != 0 {
                     x += command_args.xoff as f32;
                 }