diff --git a/src/bin/render_lines_gui.rs b/src/bin/render_lines_gui.rs index 154a7f3..ea5b3e9 100644 --- a/src/bin/render_lines_gui.rs +++ b/src/bin/render_lines_gui.rs @@ -2,21 +2,19 @@ //! A clone of the `laser_frame_stream.rs` example that allows for configuring laser settings via a //! UI. -use bevy::math::Mat3; -use bevy_nannou::prelude::DARK_GRAY; +// use bevy_nannou::prelude::DARK_GRAY; // use nannou::lyon::geom::euclid::Transform2D; use nannou::{geom::Rect, math::map_range as nannou_map_range}; use nannou::prelude::*; -use nannou_egui::egui::emath::inverse_lerp; +// use nannou_egui::egui::emath::inverse_lerp; use nannou_egui::{self, egui, Egui}; use nannou_laser::DacId; -use nannou_laser::{self as laser, util::map_range}; +use nannou_laser::{self as laser}; use serde_json::Result; -use serde::{Serialize,Deserialize}; -use trap_rust::trap::filters::{Filter, PointFilters}; +use trap_rust::trap::filters::PointFilters; use trap_rust::trap::laser::{LaserPoints, TMP_DESK_CLUBMAX}; use trap_rust::trap::tracks::CoordinateSpace; -use trap_rust::trap::{laser::{apply_homography_matrix, python_cv_h_into_mat3, LaserModel, TMP_PYTHON_LASER_H, DacConfig}, tracks::{RenderableLines}}; +use trap_rust::trap::{laser::{python_cv_h_into_mat3, LaserModel, TMP_PYTHON_LASER_H, DacConfig}, tracks::{RenderableLines}}; use zmq::Socket; use std::sync::{mpsc, Arc}; use std::time::{Instant, Duration}; @@ -27,11 +25,17 @@ fn main() { nannou::app(model).update(update).run(); } -struct Model { +pub struct StreamConfig{ + pub stream: laser::FrameStream, + pub config: DacConfig, +} +type StreamConfigMap = HashMap; + +struct GuiModel { // A handle to the laser API used for spawning streams and detecting DACs. laser_api: Arc, // All of the live stream handles. - laser_streams: Vec>, + laser_streams: StreamConfigMap, // A copy of the state that will live on the laser thread so we can present a GUI. laser_model: LaserModel, // A copy of the laser settings so that we can control them with the GUI. @@ -48,6 +52,7 @@ struct Model { // dimming_factor: f32, lost_alpha: f32, connected: bool, + selected_stream: Option, // canvas_transform: Translation2D, // dragging: bool, } @@ -85,7 +90,8 @@ impl Default for LaserSettings { fn setup_zmq() -> Socket{ - let url = "tcp://100.109.175.82:99174"; + // let url = "tcp://100.109.175.82:99174"; + let url = "tcp://127.0.0.1:99174"; let context = zmq::Context::new(); let subscriber = context.socket(zmq::SUB).unwrap(); subscriber.set_conflate(true).unwrap(); // only keep latest entry @@ -101,7 +107,7 @@ fn setup_zmq() -> Socket{ // fn zmq_receive(subscriber: &Socket, laser_streams: &Vec>) { /// Receive items if available on the queue and update Model with the new data -fn zmq_receive(model: &mut Model) { +fn zmq_receive(model: &mut GuiModel) { let subscriber = &model.zmq; let mut items = [ subscriber.as_poll_item(zmq::POLLIN) @@ -149,10 +155,10 @@ fn zmq_receive(model: &mut Model) { // println!("receive {}", lines.lines.len()); - for laser_stream in (&model.laser_streams).into_iter() { + for (_dac, stream_config) in (&model.laser_streams).into_iter() { // let lines = get_laser_lines(version); let lines_for_laser: RenderableLines = lines.clone(); - let sending = laser_stream.send(move |laser| { + let sending = stream_config.stream.send(move |laser| { let laser_lines: RenderableLines = lines_for_laser; laser.current_lines = laser_lines; }); @@ -179,6 +185,7 @@ fn zmq_receive(model: &mut Model) { type DacConfigMap = HashMap; + // Some hardcoded config. Not spending time on reading/writing config atm. fn get_dac_configs() -> DacConfigMap{ @@ -225,10 +232,27 @@ fn get_dac_configs() -> DacConfigMap{ filters: PointFilters::default(), } ); + dac_configs.insert( + DacId::EtherDream { + mac_address: [ + 18, + 52, + 86, + 120, + 154, + 188, + ], + }, + DacConfig{ + name: "Emulator".into(), + homography: python_cv_h_into_mat3(TMP_DESK_CLUBMAX), + filters: PointFilters::default(), + } + ); dac_configs } -fn model(app: &App) -> Model { +fn model(app: &App) -> GuiModel { // Create a window to receive keyboard events. let w_id_lasersettings = app .new_window() @@ -300,7 +324,7 @@ fn model(app: &App) -> Model { }); // We'll use a `Vec` to collect laser streams as they appear. - let laser_streams = vec![]; + let laser_streams = HashMap::new(); //vec![]; // A user-interface to tweak the settings. let window = app.window(w_id_lasersettings).unwrap(); @@ -310,7 +334,7 @@ fn model(app: &App) -> Model { let current_lines = RenderableLines::new(); //Vec::new(); - Model { + GuiModel { laser_api, laser_settings, laser_model, @@ -323,84 +347,12 @@ fn model(app: &App) -> Model { lost_alpha: 1., connected: true, per_laser_config: get_dac_configs(), + selected_stream: None, // canvas_transform: Transform2D // dimming_factor: 1., } } -// Draw lines or points based on the `DrawMode`. -// fn add_points(points: I, scale: f32, frame: &mut laser::Frame) -// where -// I: IntoIterator, -// I::Item: AsRef, -// { -// let points = points.into_iter().map(|p| { -// let mut p = p.as_ref().clone(); -// p.position[0] *= scale; -// p.position[1] *= scale; -// p -// }); -// frame.add_lines(points); -// } - -const LASER_MIN: f32 = -1.0; -const LASER_MAX: f32 = 1.0; -fn within_laser_bounds(position: &[f32; 2]) -> bool { - !(position[0] < LASER_MIN || position[0] > LASER_MAX || position[1] < LASER_MIN || position[1] > LASER_MAX) -} -fn interpolate_to_bounds(a: &[f32;2], b: &[f32;2]) { - //let t_a = inverse_lerp(a) -} - -// From ChatGTP: Lian-Barsky Algorithm for line segment cropping -fn clip_line_to_bounds( - p1: &[f32; 2], - p2: &[f32; 2], -) -> Option<([f32; 2], [f32; 2])> { - let min = [LASER_MIN, LASER_MIN]; - let max = [LASER_MAX, LASER_MAX]; - let dx = p2[0] - p1[0]; - let dy = p2[1] - p1[1]; - - let mut t0 = 0.0; - let mut t1 = 1.0; - - let checks = [ - (-dx, p1[0] - min[0]), // Left - (dx, max[0] - p1[0]), // Right - (-dy, p1[1] - min[1]), // Bottom - (dy, max[1] - p1[1]), // Top - ]; - - for (p, q) in checks { - if p == 0.0 { - if q < 0.0 { - return None; // Line is parallel and outside - } - } else { - let r = q / p; - if p < 0.0 { - if r > t1 { - return None; - } else if r > t0 { - t0 = r; - } - } else { - if r < t0 { - return None; - } else if r < t1 { - t1 = r; - } - } - } - } - - let clipped_p1 = [p1[0] + t0 * dx, p1[1] + t0 * dy]; - let clipped_p2 = [p1[0] + t1 * dx, p1[1] + t1 * dy]; - - Some((clipped_p1, clipped_p2)) -} - fn laser_frame_producer(model: &mut LaserModel, frame: &mut laser::Frame){ let points: LaserPoints = (&model.current_lines).into(); @@ -415,99 +367,13 @@ fn laser_frame_producer(model: &mut LaserModel, frame: &mut laser::Frame){ frame.add_lines(new_laser_points); return; - - // dbg!(&model.config.name); - - - // let mut new_points = Vec::new(); - // let projected_positions: Vec<[f32;2]> = points.iter().map(|point| { - // let p = point.position; - // let new_position = match space { - // CoordinateSpace::World => apply_homography_matrix(model.config.homography, &p), - // CoordinateSpace::Laser => p, - // _ => panic!("Invalid coordinate space"), - - // }; - // // let new_position = apply_homography_matrix(LASER_H, &p); - // // let s = 1.; // when using TMP_PYTHON_LASER_H_FOR_NANNOU -- doesn't work? - // let s = 0xFFF as f32 / 2.; // when using TMP_PYTHON_LASER_H - // [new_position[0]/s - 1., new_position[1]/s - 1.] - // }).collect(); - - // for (id, position ) in projected_positions.iter().enumerate() { - // let point = points[id] ; - - // let mut new_positions: Vec<[f32;2]> = Vec::new(); - - // // const LASER_MIN: f32 = -1.0; - // // const LASER_MAX: f32 = 1.0; - // if !within_laser_bounds(position) { - // let mut either = false; - // if id > 0 { - // let prev_position = projected_positions[id-1]; - // if within_laser_bounds(&prev_position) { - // either = true; - // // interpolate with prev - // let clip = clip_line_to_bounds(&prev_position, position); - // if let Some((p1, p2)) = clip { - // new_positions.push(p1); - // new_positions.push(p2); - // } - // } - // } - - // if id < (projected_positions.len()-1) { - // let next_position = projected_positions[id+1]; - // if within_laser_bounds(&next_position) { - // either = true; - // // interpolate with next - // let clip = clip_line_to_bounds(position, &next_position); - // if let Some((p1, p2)) = clip { - // new_positions.push(p1); - // new_positions.push(p2); - // } - // } - // } - - // if !either { - // // if neither prev nor next is withint bounds, point can be ditched - // continue; - // } - // } else { - // new_positions.push(position.clone()); - // } - // let mut color = point.color.clone(); - // if model.dimming < 1.0 { - // color[0] *= model.dimming; - // color[1] *= model.dimming; - // color[2] *= model.dimming; - // } - - // for position in new_positions { - // // let pos: [f32; 2] = position.clone(); - // let new_point = laser::Point { - // position, - // color, - // .. point.clone() - // }; - // new_points.push(new_point); - // } - - // } - - // if new_points.len() < pointno { - // println!("Cropped Points {} (was: {})", new_points.len(), pointno); - // } - - // // println!("{:?}", new_points); - // frame.add_lines(new_points); } -fn raw_window_event(_app: &App, model: &mut Model, event: &nannou::winit::event::WindowEvent) { +fn raw_window_event(_app: &App, model: &mut GuiModel, event: &nannou::winit::event::WindowEvent) { model.egui.handle_raw_event(event); } -fn update(_app: &App, model: &mut Model, update: Update) { +fn update(_app: &App, model: &mut GuiModel, update: Update) { // First, check for new laser DACs. for dac in model.dac_rx.try_recv() { println!("Detected DAC {:?}!", dac.id()); @@ -522,53 +388,69 @@ fn update(_app: &App, model: &mut Model, update: Update) { let stream = model .laser_api .new_frame_stream(model.laser_model.with_config(config), laser_frame_producer) - .detected_dac(dac) + .detected_dac(dac.clone()) .build() .expect("failed to establish stream with newly detected DAC"); - model.laser_streams.push(stream); + model.laser_streams.insert(dac.id(), StreamConfig{ stream, config: config.clone() }); } // Check if any streams have dropped out (e.g network issues, DAC turned off) and attempt to // start them again. let mut dropped = vec![]; - for (i, stream) in model.laser_streams.iter().enumerate() { - if stream.is_closed() { - dropped.push(i); - } - } - for i in dropped.into_iter().rev() { - let stream = model.laser_streams.remove(i); - let dac = stream - .dac() - .expect("`dac` returned `None` even though one was specified during stream creation"); - let res = stream - .close() - .expect("stream was unexpectedly already closed from another stream handle") - .expect("failed to join stream thread"); - if let Err(err) = res { - eprintln!("Stream closed due to an error: {}", err); - } - println!("attempting to restart stream with DAC {:?}", dac.id()); - match model - .laser_api - .new_frame_stream(model.laser_model.clone(), laser_frame_producer) - .detected_dac(dac) - .build() - { - Err(err) => eprintln!("failed to restart stream: {}", err), - Ok(stream) => model.laser_streams.push(stream), + for (dac_id, stream_config) in model.laser_streams.iter() { + if stream_config.stream.is_closed() { + dropped.push(dac_id.clone()); } } + for dac_id in dropped.into_iter().rev() { + // let stream = ; + let s = model.laser_streams.remove(&dac_id); + + if model.selected_stream == Some(dac_id){ + model.selected_stream = None; + } + + if let Some(stream_config) = s{ + let dac = stream_config.stream + .dac() + .expect("`dac` returned `None` even though one was specified during stream creation"); + let res = stream_config.stream + .close() + .expect("stream was unexpectedly already closed from another stream handle") + .expect("failed to join stream thread"); + if let Err(err) = res { + eprintln!("Stream closed due to an error: {}", err); + } + // TODO: keeps looping on disconnect. + println!("attempting to restart stream with DAC {:?}", dac.id()); + let dac_id = dac.id(); + match model + .laser_api + .new_frame_stream(model.laser_model.clone(), laser_frame_producer) + .detected_dac(dac) + .build() + { + Err(err) => eprintln!("failed to restart stream: {}", err), + Ok(stream) => {model.laser_streams.insert(dac_id, StreamConfig{stream, config: stream_config.config});}, + } + } + + } + + // check if new messages have arrived. Update the model with new data. zmq_receive(model); // Update the GUI. - let Model { + let GuiModel { ref mut egui, - ref laser_streams, + ref mut laser_streams, ref mut laser_model, ref mut laser_settings, + ref mut per_laser_config, + ref mut selected_stream, + ref mut current_lines, .. } = *model; @@ -585,34 +467,21 @@ fn update(_app: &App, model: &mut Model, update: Update) { } ui.heading("Laser Points"); - + ui.separator(); + + ui.add(egui::Label::new(format!("Lines {}", current_lines.lines.len()))); + ui.add(egui::Label::new(format!("Points {}", current_lines.point_count()))); - ui.add(egui::Label::new(format!("Lines {}", model.current_lines.lines.len()))); - ui.add(egui::Label::new(format!("Points {}", model.current_lines.point_count()))); - - ui.heading("Laser Settings"); - - if ui - .add(egui::Slider::new(&mut model.laser_model.dimming, 0.0..=1.).text("Dimming")) - .changed() - { - for laser_stream in laser_streams { - let factor = model.laser_model.dimming; - // let lines = get_laser_lines(version); - laser_stream.send(move |laser| { - laser.dimming = factor; - }).unwrap(); - } - } + ui.heading("General settings"); if ui .add(egui::Slider::new(&mut laser_settings.point_hz, 1_000..=50_000).text("DAC PPS")) .changed() { let hz = laser_settings.point_hz; - for stream in laser_streams { - stream.set_point_hz(hz).ok(); + for (_dac_id, stream) in laser_streams.iter() { + stream.stream.set_point_hz(hz).ok(); } } if ui @@ -620,20 +489,20 @@ fn update(_app: &App, model: &mut Model, update: Update) { .changed() { let latency = laser_settings.latency_points; - for stream in laser_streams { - stream.set_latency_points(latency).ok(); + for (_dac_id, stream) in laser_streams.iter() { + stream.stream.set_latency_points(latency).ok(); } } if ui .add(egui::Slider::new(&mut laser_settings.frame_hz, 1..=120).text("Target FPS")) .changed() - { - let hz = laser_settings.frame_hz; - for stream in laser_streams { - stream.set_frame_hz(hz).ok(); - } + { + let hz = laser_settings.frame_hz; + for (_dac_id, stream) in laser_streams.iter() { + stream.stream.set_frame_hz(hz).ok(); + } } - + ui.separator(); ui.heading("Laser Path Interpolation"); @@ -642,34 +511,34 @@ fn update(_app: &App, model: &mut Model, update: Update) { .checkbox(&mut laser_settings.enable_optimisations, "Optimize Path") .changed() { - for stream in laser_streams { - stream + for (_dac_id, stream_config) in laser_streams.iter() { + stream_config.stream .enable_optimisations(laser_settings.enable_optimisations) .ok(); } } if ui - .add( - egui::Slider::new(&mut laser_settings.distance_per_point, 0.01..=1.0) + .add( + egui::Slider::new(&mut laser_settings.distance_per_point, 0.01..=1.0) .text("Distance Per Point"), - ) + ) .changed() { let distance = laser_settings.distance_per_point; - for stream in laser_streams { - stream.set_distance_per_point(distance).ok(); + for (_dac_id, stream) in laser_streams.iter() { + stream.stream.set_distance_per_point(distance).ok(); } } if ui .add( egui::Slider::new(&mut laser_settings.blank_delay_points, 0..=32) - .text("Blank Delay (Points)"), + .text("Blank Delay (Points)"), ) .changed() { let delay = laser_settings.blank_delay_points; - for stream in laser_streams { - stream.set_blank_delay_points(delay).ok(); + for (_dac_id, stream) in laser_streams.iter() { + stream.stream.set_blank_delay_points(delay).ok(); } } let mut degrees = rad_to_deg(laser_settings.radians_per_point); @@ -679,39 +548,119 @@ fn update(_app: &App, model: &mut Model, update: Update) { { let radians = deg_to_rad(degrees); laser_settings.radians_per_point = radians; - for stream in laser_streams { - stream.set_radians_per_point(radians).ok(); + for (_dac_id, stream) in laser_streams.iter() { + stream.stream.set_radians_per_point(radians).ok(); } } - ui.heading("Connected DACs"); - - if laser_streams.len() < 1 { - ui.label("no streams"); + ui.separator(); + ui.heading("Laser specific settings"); + + if laser_streams.is_empty() { + ui.label("No dacs available"); + } else { + ui.horizontal(|ui| { + for (dac_id, _stream) in laser_streams.iter() { + ui.selectable_value( + selected_stream, + Some(dac_id.clone()), + if let Some(config) = per_laser_config.get(&dac_id) { config.name.clone() } else { "DAC".into() } + ); + } + }); } - - for stream in laser_streams { - let dac: laser::DetectedDac = stream - .dac() - .expect("`dac` returned `None` even though one was specified during stream creation"); - ui.add(egui::Label::new(format!("{:?}", dac.id()))); - + + if let Some(selected_stream_value) = selected_stream { + + ui.separator(); + ui.add(egui::Label::new(format!("{:?}", selected_stream_value))); + + let stream_config: &mut StreamConfig = laser_streams.get_mut(&selected_stream_value).expect("Selected stream not found in configs"); + if ui - // todo : from custom dac config: - .add(egui::Slider::new(&mut model.laser_model.dimming, 0.0..=1.).text("Dimming")) - .changed() - { - for laser_stream in laser_streams { - let factor = model.laser_model.dimming; - // let lines = get_laser_lines(version); - laser_stream.send(move |laser| { - // laser: LaserModel - laser.config.filters.dim.intensity = factor; - // laser.dimming = factor; + .add(egui::Slider::new(&mut stream_config.config.filters.dim.intensity, 0.0..=1.).text("Dimming")) + .changed() + { + let factor = stream_config.config.filters.dim.intensity; + stream_config.stream.send(move |laser_model: &mut LaserModel| { + laser_model.config.filters.dim.intensity = factor; }).unwrap(); } - } + + if ui + .add(egui::Slider::new(&mut stream_config.config.filters.scale.factor, 0.0..=2.).text("Scale")) + .changed() + { + let factor = stream_config.config.filters.scale.factor; + stream_config.stream.send(move |laser_model: &mut LaserModel| { + laser_model.config.filters.scale.factor = factor; + }).unwrap(); + } + + + if ui + .add(egui::Slider::new(&mut stream_config.config.filters.pincushion.k_x, -1.0..=1.).text("Pincushion x")) + .changed() + { + let factor = stream_config.config.filters.pincushion.k_x; + stream_config.stream.send(move |laser_model: &mut LaserModel| { + laser_model.config.filters.pincushion.k_x = factor; + }).unwrap(); + } + + if ui + .add(egui::Slider::new(&mut stream_config.config.filters.pincushion.k_y, -1.0..=1.).text("Pincushion y")) + .changed() + { + let factor = stream_config.config.filters.pincushion.k_y; + stream_config.stream.send(move |laser_model: &mut LaserModel| { + laser_model.config.filters.pincushion.k_y = factor; + }).unwrap(); + } + + + if ui + .checkbox(&mut stream_config.config.filters.crop.enabled ,"Crop") + .changed() + { + let enabled = stream_config.config.filters.crop.enabled; + stream_config.stream.send(move |laser_model: &mut LaserModel| { + laser_model.config.filters.crop.enabled = enabled; + }).unwrap(); + } + + + + // ui.heading("Connected DACs"); + + // if laser_streams.is_empty() { + // ui.label("no streams"); + // } + + // for (_dac_id, stream_config) in laser_streams.iter_mut() { + // let dac: laser::DetectedDac = stream_config.stream + // .dac() + // .expect("`dac` returned `None` even though one was specified during stream creation"); + // ui.add(egui::Label::new(format!("{:?}", dac.id()))); + + // if ui + // // todo : from custom dac config: + // .add(egui::Slider::new(&mut stream_config.config.filters.dim.intensity, 0.0..=1.).text("Dimming")) + // .changed() + // { + // for (_dac_id, laser_stream) in laser_streams.iter() { + // let factor = model.laser_model.dimming; + // // let lines = get_laser_lines(version); + // laser_stream.stream.send(move |laser| { + // // laser: LaserModel + // laser.config.filters.dim.intensity = factor; + // // laser.dimming = factor; + // }).unwrap(); + // } + // } + + // } //if egui::ComboBox::from_label("Homography") // .selected_text(format!("{radio:?}")) @@ -722,23 +671,25 @@ fn update(_app: &App, model: &mut Model, update: Update) { // }) // .changed() { // let sending = laser_stream.send(move |laser| { - // let laser_lines: RenderableLines = lines_for_laser; - // laser.current_lines = laser_lines; - // }); - // if let Err(e) = sending { - // println!("Error sending to laser! {e:?}"); - // } - // }; + // let laser_lines: RenderableLines = lines_for_laser; + // laser.current_lines = laser_lines; + // }); + // if let Err(e) = sending { + // println!("Error sending to laser! {e:?}"); + // } + // }; + } else { + ui.label("Select a DAC"); } }); } -fn view_laser_settings(_app: &App, model: &Model, frame: Frame) { +fn view_laser_settings(_app: &App, model: &GuiModel, frame: Frame) { model.egui.draw_to_frame(&frame).unwrap(); } -fn view_line_canvas(app: &App, model: &Model, frame: Frame) { +fn view_line_canvas(app: &App, model: &GuiModel, frame: Frame) { // get canvas to draw on let draw = app.draw(); @@ -832,15 +783,15 @@ fn style() -> egui::Style { style } -fn mouse_moved(_app: &App, _model: &mut Model, _pos: Point2) { +fn mouse_moved(_app: &App, _model: &mut GuiModel, _pos: Point2) { } -fn mouse_pressed(_app: &App, _model: &mut Model, _button: MouseButton) { +fn mouse_pressed(_app: &App, _model: &mut GuiModel, _button: MouseButton) { // _model.dragging } -fn mouse_released(_app: &App, _model: &mut Model, _button: MouseButton) {} +fn mouse_released(_app: &App, _model: &mut GuiModel, _button: MouseButton) {} -fn mouse_wheel(_app: &App, _model: &mut Model, _dt: MouseScrollDelta, _phase: TouchPhase) { +fn mouse_wheel(_app: &App, _model: &mut GuiModel, _dt: MouseScrollDelta, _phase: TouchPhase) { // canvas zoom } diff --git a/src/trap/filters.rs b/src/trap/filters.rs index c90aa04..501aba8 100644 --- a/src/trap/filters.rs +++ b/src/trap/filters.rs @@ -18,6 +18,7 @@ pub struct HomographyFilter { #[derive(Serialize, Deserialize, Clone)] pub struct CropFilter { + pub enabled: bool } #[derive(Serialize, Deserialize, Clone)] @@ -25,6 +26,11 @@ pub struct DimFilter { pub intensity: f32 } +#[derive(Serialize, Deserialize, Clone)] +pub struct ScaleFilter { + pub factor: f32 +} + #[derive(Serialize, Deserialize, Clone)] pub struct PincushionFilter { pub k_x: f32, @@ -35,17 +41,19 @@ pub struct PincushionFilter { // TODO consider moving to struct? pub enum PointFilter { Homography(HomographyFilter), - Crop(CropFilter), Dim(DimFilter), + Scale(ScaleFilter), Pincushion(PincushionFilter), + Crop(CropFilter), } pub struct PointFilterList(Vec); // deprecated #[derive(Serialize, Deserialize, Clone)] pub struct PointFilters{ - pub homography: HomographyFilter, pub dim: DimFilter, + pub homography: HomographyFilter, + pub scale: ScaleFilter, pub pincushion: PincushionFilter, pub crop: CropFilter, } @@ -68,6 +76,7 @@ impl PointFilters { pub fn apply(&self, points: &LaserPoints) -> LaserPoints{ let mut p = self.dim.apply(points); p = self.homography.apply(&p); + p = self.scale.apply(&p); p = self.pincushion.apply(&p); p = self.crop.apply(&p); p @@ -85,8 +94,9 @@ impl Default for PointFilters { Self { homography: HomographyFilter::default(), dim: DimFilter{intensity: 0.5}, + scale: ScaleFilter { factor: 1. }, pincushion: PincushionFilter{k_x: 0., k_y: 0.}, - crop: CropFilter{}, + crop: CropFilter{ enabled: true }, } } } @@ -186,6 +196,14 @@ fn clip_line_to_bounds( impl Filter for CropFilter { fn apply(&self, points: &LaserPoints) -> LaserPoints { + if !self.enabled { + // don't modify if disabled + return LaserPoints{ + points: points.points.clone(), + space: points.space, + }; + } + let space = points.space; let mut new_points = Vec::new(); @@ -266,6 +284,23 @@ impl Filter for DimFilter { } } +impl Filter for ScaleFilter { + fn apply(&self, points: &LaserPoints) -> LaserPoints { + let new_points = points.points.iter().map(|point| { + let mut position = point.position.clone(); + if self.factor != 1.0 { + position[0] *= self.factor; + position[1] *= self.factor; + } + Point::new(position, point.color) + }).collect(); + LaserPoints { + points: new_points, + space: points.space + } + } +} + impl Filter for PincushionFilter { // The formula for pincushion distortion is: r_u = r_d * (1 + k * r_d^2) // see also https://stackoverflow.com/a/6227310 @@ -273,14 +308,14 @@ impl Filter for PincushionFilter { // becomes trivial fn apply(&self, points: &LaserPoints) -> LaserPoints{ let space = points.space; - dbg!(&space); + // dbg!(&space); // assert!(!matches!(space, CoordinateSpace::Laser)); let projected_positions: Vec = points.points.iter().map(|point| { let p = point.position; let new_position = [ p[0] * (1. + self.k_x * p[0].powi(2)), - p[1] * (1. + self.k_x * p[1].powi(2)) + p[1] * (1. + self.k_y * p[1].powi(2)) ]; laser::Point {