From 9aefe4f7f6705656a100d87ac58907cd99713bae Mon Sep 17 00:00:00 2001 From: Ruben van de Ven Date: Fri, 27 Jun 2025 14:16:17 +0200 Subject: [PATCH] basic filter implementation --- src/bin/render_lines_gui.rs | 190 +++++++++++++---------- src/trap/filters.rs | 298 ++++++++++++++++++++++++++++++++++++ src/trap/laser.rs | 15 +- src/trap/mod.rs | 3 +- src/trap/tracks.rs | 10 +- src/trap/zmqplugin.rs | 2 +- 6 files changed, 431 insertions(+), 87 deletions(-) create mode 100644 src/trap/filters.rs diff --git a/src/bin/render_lines_gui.rs b/src/bin/render_lines_gui.rs index a4d0603..154a7f3 100644 --- a/src/bin/render_lines_gui.rs +++ b/src/bin/render_lines_gui.rs @@ -13,14 +13,16 @@ use nannou_laser::DacId; use nannou_laser::{self as laser, util::map_range}; use serde_json::Result; use serde::{Serialize,Deserialize}; -use trap_rust::trap::laser::TMP_DESK_CLUBMAX; +use trap_rust::trap::filters::{Filter, 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::{LaserPoints, RenderableLines}}; +use trap_rust::trap::{laser::{apply_homography_matrix, 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}; use std::collections::HashMap; + fn main() { nannou::app(model).update(update).run(); } @@ -185,7 +187,8 @@ fn get_dac_configs() -> DacConfigMap{ DacId::Helios { id: 926298163 }, DacConfig{ name: "Helios#1".into(), - homography: python_cv_h_into_mat3(TMP_PYTHON_LASER_H) + homography: python_cv_h_into_mat3(TMP_PYTHON_LASER_H), + filters: PointFilters::default(), } ); dac_configs.insert( @@ -201,7 +204,8 @@ fn get_dac_configs() -> DacConfigMap{ }, DacConfig{ name: "ED - 192.168.8.101".into(), - homography: python_cv_h_into_mat3(TMP_DESK_CLUBMAX) + homography: python_cv_h_into_mat3(TMP_DESK_CLUBMAX), + filters: PointFilters::default(), } ); dac_configs.insert( @@ -217,7 +221,8 @@ fn get_dac_configs() -> DacConfigMap{ }, DacConfig{ name: "ED - 192.168.9.101".into(), - homography: python_cv_h_into_mat3(TMP_DESK_CLUBMAX) + homography: python_cv_h_into_mat3(TMP_DESK_CLUBMAX), + filters: PointFilters::default(), } ); dac_configs @@ -400,92 +405,102 @@ fn laser_frame_producer(model: &mut LaserModel, frame: &mut laser::Frame){ let points: LaserPoints = (&model.current_lines).into(); let space = &model.current_lines.space; - let pointno = points.len(); + let pointno = points.points.len(); + + let new_points = model.config.filters.apply(&points); + let new_laser_points = new_points.points; + if new_laser_points.len() < pointno { + println!("Cropped Points {} (was: {})", new_laser_points.len(), pointno); + } + + 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 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(); + // }; + // // 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] ; + // for (id, position ) in projected_positions.iter().enumerate() { + // let point = points[id] ; - let mut new_positions: Vec<[f32;2]> = Vec::new(); + // 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); - } - } - } + // // 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 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; - } + // 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); - } + // 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); - } + // if new_points.len() < pointno { + // println!("Cropped Points {} (was: {})", new_points.len(), pointno); + // } - // println!("{:?}", new_points); - frame.add_lines(new_points); + // // println!("{:?}", new_points); + // frame.add_lines(new_points); } fn raw_window_event(_app: &App, model: &mut Model, event: &nannou::winit::event::WindowEvent) { @@ -676,11 +691,28 @@ fn update(_app: &App, model: &mut Model, update: Update) { } for stream in laser_streams { - let dac = stream + 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 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; + }).unwrap(); + } + } + + //if egui::ComboBox::from_label("Homography") // .selected_text(format!("{radio:?}")) // .show_ui(ui, |ui| { diff --git a/src/trap/filters.rs b/src/trap/filters.rs new file mode 100644 index 0000000..c90aa04 --- /dev/null +++ b/src/trap/filters.rs @@ -0,0 +1,298 @@ +use bevy::prelude::*; // for glam::f32::Mat3 + +use crate::trap::{laser::{apply_homography_matrix, LaserPoints}, tracks::CoordinateSpace}; + +use nannou_laser::{self as laser, Point}; +use serde::{Deserialize, Serialize}; + +pub trait Filter { + // fn set_config(&self) + // fn set_config(&self) + fn apply(&self, points: &LaserPoints) -> LaserPoints; +} + +#[derive(Serialize, Deserialize, Clone)] +pub struct HomographyFilter { + pub homography_matrix: Mat3 +} + +#[derive(Serialize, Deserialize, Clone)] +pub struct CropFilter { +} + +#[derive(Serialize, Deserialize, Clone)] +pub struct DimFilter { + pub intensity: f32 +} + +#[derive(Serialize, Deserialize, Clone)] +pub struct PincushionFilter { + pub k_x: f32, + pub k_y: f32 +} + +#[derive(Serialize, Deserialize, Clone)] +// TODO consider moving to struct? +pub enum PointFilter { + Homography(HomographyFilter), + Crop(CropFilter), + Dim(DimFilter), + Pincushion(PincushionFilter), +} + +pub struct PointFilterList(Vec); // deprecated + +#[derive(Serialize, Deserialize, Clone)] +pub struct PointFilters{ + pub homography: HomographyFilter, + pub dim: DimFilter, + pub pincushion: PincushionFilter, + pub crop: CropFilter, +} + +// list of enums deprecated in favour of struct +// impl Default for PointFilterList { +// fn default() -> Self { +// // let crop_filter = CropFilter{}; +// Self ( +// vec![ +// PointFilter::Dim(DimFilter{intensity: 0.5}), +// PointFilter::Pincushion(PincushionFilter{k_x: 0., k_y: 0.}), +// PointFilter::Crop(CropFilter{}), +// ] +// ) +// } +// } + +impl PointFilters { + pub fn apply(&self, points: &LaserPoints) -> LaserPoints{ + let mut p = self.dim.apply(points); + p = self.homography.apply(&p); + p = self.pincushion.apply(&p); + p = self.crop.apply(&p); + p + } + + pub fn with_homography(mut self, h: Mat3) -> Self{ + self.homography.homography_matrix = h; + self + } +} + +impl Default for PointFilters { + fn default() -> Self { + // let crop_filter = CropFilter{}; + Self { + homography: HomographyFilter::default(), + dim: DimFilter{intensity: 0.5}, + pincushion: PincushionFilter{k_x: 0., k_y: 0.}, + crop: CropFilter{}, + } + } +} + + +impl Filter for HomographyFilter { + fn apply(&self, points: &LaserPoints) -> LaserPoints{ + let space = points.space; + + let projected_positions: Vec = points.points.iter().map(|point| { + let p = point.position; + let new_position = match space { + CoordinateSpace::World => apply_homography_matrix(self.homography_matrix, &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 + let normalised_pos: [f32;2] = [new_position[0]/s - 1., new_position[1]/s - 1.]; + laser::Point { + position: normalised_pos, + .. point.clone() + } + + }).collect(); + LaserPoints{ + points: projected_positions, + space: CoordinateSpace::Laser + } + } +} + +impl Default for HomographyFilter{ + fn default() -> Self { + return Self { homography_matrix: Mat3::IDENTITY } + } +} + +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) +} + +// 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)) +} + + +impl Filter for CropFilter { + + fn apply(&self, points: &LaserPoints) -> LaserPoints { + let space = points.space; + + let mut new_points = Vec::new(); + + for (id, point) in points.points.iter().enumerate() { + + 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(&point.position) { + let mut either = false; + if id > 0 { + let prev_position = points.points[id-1].position; + if within_laser_bounds(&prev_position) { + either = true; + // interpolate with prev + let clip = clip_line_to_bounds(&prev_position, &point.position); + if let Some((p1, p2)) = clip { + new_positions.push(p1); + new_positions.push(p2); + } + } + } + + if id < (points.points.len()-1) { + let next_position = points.points[id+1].position; + if within_laser_bounds(&next_position) { + either = true; + // interpolate with next + let clip = clip_line_to_bounds(&point.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(point.position.clone()); + } + + for position in new_positions { + // let pos: [f32; 2] = position.clone(); + let new_point = laser::Point { + position, + .. point.clone() + }; + new_points.push(new_point); + } + } + LaserPoints{ + points: new_points, + space + } + } +} + +impl Filter for DimFilter { + fn apply(&self, points: &LaserPoints) -> LaserPoints { + let new_points = points.points.iter().map(|point| { + let mut color = point.color.clone(); + if self.intensity != 1.0 { + color[0] *= self.intensity; + color[1] *= self.intensity; + color[2] *= self.intensity; + } + Point::new(point.position, 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 + // As points in laser space center around 0,0, calculating from the center + // becomes trivial + fn apply(&self, points: &LaserPoints) -> LaserPoints{ + let space = points.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)) + ]; + + laser::Point { + position: new_position, + .. point.clone() + } + + }).collect(); + + LaserPoints{ + points: projected_positions, + space + } + } +} diff --git a/src/trap/laser.rs b/src/trap/laser.rs index 247d18e..f259eb5 100644 --- a/src/trap/laser.rs +++ b/src/trap/laser.rs @@ -2,7 +2,15 @@ use bevy::prelude::*; use nannou_laser as laser; use std::time::Instant; use serde::{Deserialize, Serialize}; -use super::tracks::{LaserPoints, RenderableLines}; +use crate::trap::{filters::{PointFilter, PointFilters}, tracks::CoordinateSpace}; + +use super::tracks::{RenderableLines}; + +pub struct LaserPoints{ + pub points: Vec, + pub space: CoordinateSpace +} + // homography for laserworld in studio pub const TMP_PYTHON_LASER_H: [[f32;3];3] = [[ 2.47442963e+02, -7.01714050e+01, -9.71749119e+01], @@ -78,7 +86,8 @@ pub struct DacConfig{ // #[serde(with = "DacIdSerializable")] // id: DacId, pub name: String, - pub homography: Mat3 + pub homography: Mat3, + pub filters: PointFilters } const LASER_H: Mat3 = python_cv_h_into_mat3(TMP_PYTHON_LASER_H); @@ -89,6 +98,6 @@ impl Default for DacConfig{ fn default() -> DacConfig{ //DacConfig { name: "Unknown".into(), homography: Mat3::IDENTITY } // DacConfig { name: "Unknown".into(), homography: LASER_H_CM } - DacConfig { name: "Unknown".into(), homography: LASER_H } + DacConfig { name: "Unknown".into(), homography: LASER_H, filters: PointFilters::default().with_homography(LASER_H) } } } diff --git a/src/trap/mod.rs b/src/trap/mod.rs index fee4a83..5c62b1e 100644 --- a/src/trap/mod.rs +++ b/src/trap/mod.rs @@ -9,4 +9,5 @@ pub mod tracks; pub mod shapes; -pub mod laser; \ No newline at end of file +pub mod laser; +pub mod filters; \ No newline at end of file diff --git a/src/trap/tracks.rs b/src/trap/tracks.rs index c253856..35cd774 100644 --- a/src/trap/tracks.rs +++ b/src/trap/tracks.rs @@ -4,6 +4,8 @@ use std::time::Instant; use nannou_laser as laser; use serde_repr::*; +use crate::trap::laser::LaserPoints; + #[derive(Serialize,Deserialize)] pub struct Frame { pub tracks: std::collections::HashMap @@ -68,7 +70,7 @@ impl RenderableLine { } // see also trap/lines.py for matching values -#[derive(Clone, Debug, Serialize_repr, Deserialize_repr)] +#[derive(Clone, Debug, Serialize_repr, Deserialize_repr, Copy)] #[repr(u8)] pub enum CoordinateSpace { Image = 1, @@ -140,7 +142,6 @@ impl From<&Track> for RenderableLines{ } // TODO migrate to euclid::Point2D -pub type LaserPoints = Vec; impl From<&RenderableLines> for LaserPoints { // much like nannou_laser::stream::frame::add_lines() @@ -160,7 +161,10 @@ impl From<&RenderableLines> for LaserPoints { } points.extend(line.points.iter().map(|p| laser::Point::from(p))); } - points + Self{ + points, + space: CoordinateSpace::World + } } } diff --git a/src/trap/zmqplugin.rs b/src/trap/zmqplugin.rs index 93e056b..669741a 100644 --- a/src/trap/zmqplugin.rs +++ b/src/trap/zmqplugin.rs @@ -2,7 +2,7 @@ use zmq::Socket; use serde_json::Result; use bevy::{ecs::system::SystemState, prelude::*, render::Render}; use std::num::NonZero; -use super::{laser::{LaserApi, LaserTimer}, tracks::{Frame, LaserPoints, RenderableLines, Track, TrackBundle}}; +use super::{laser::{LaserApi, LaserTimer}, tracks::{Frame, RenderableLines, Track, TrackBundle}}; // use trap::{Frame, Track, TrackBundle};