diff --git a/src/bin/render_tracks.rs b/src/bin/render_tracks.rs new file mode 100644 index 0000000..72eb70c --- /dev/null +++ b/src/bin/render_tracks.rs @@ -0,0 +1,216 @@ +use bevy::prelude::*; + +use bevy_nannou::prelude::*; +use bevy_nannou::NannouPlugin; +use nannou_laser::point::Rgb; +use trap_rust::trap::laser::apply_homography_matrix; +use trap_rust::trap::laser::python_cv_h_into_mat3; +use trap_rust::trap::laser::LaserApi; +use trap_rust::trap::laser::LaserModel; +use trap_rust::trap::laser::LaserTimer; +use trap_rust::trap::laser::TMP_PYTHON_LASER_H; +use trap_rust::trap::laser::TMP_PYTHON_LASER_H_FOR_NANNOU; +use trap_rust::trap::shapes::PositionAndIntensity; +use trap_rust::trap::tracks::LaserPoints; +use trap_rust::trap::tracks::RenderableLines; +use trap_rust::trap::tracks::SpawnedTime; +use trap_rust::trap::tracks::Track; +use trap_rust::trap::zmqplugin::ZmqPlugin; +// use iyes_perf_ui::PerfUiPlugin; + +use nannou_laser as laser; +use trap_rust::trap::zmqplugin::ZmqReceiveTarget; + +use std::time::Duration; + + +// pub mod trap_rust::trap; + +fn main() { + let mut app = App::new(); + // app.add_plugins((DefaultPlugins, NannouPlugin)) + app.add_plugins((DefaultPlugins, NannouPlugin)) + // .add_plugins(bevy::diagnostic::FrameTimeDiagnosticsPlugin) + // .add_plugins(bevy::diagnostic::EntityCountDiagnosticsPlugin) + // .add_plugins(bevy::diagnostic::SystemInformationDiagnosticsPlugin) + // .add_plugins(PerfUiPlugin) + .add_systems(Startup, setup) + .add_systems(Startup, setup_laser) + .add_systems(Update, update) + .add_systems(Update, exit_system) + + + .add_plugins(ZmqPlugin { + // url: "tcp://localhost:5558".to_string(), + // url: "tcp://100.109.175.82:99173".to_string(), + url: "tcp://127.0.0.1:99173".to_string(), + filter: "".to_string(), + target: ZmqReceiveTarget::TRACKS + }) + + + .run(); +} + + +// Because of world.insert_non_send_resource, this is an exclusive system +// see: https://bevy-cheatbook.github.io/programming/exclusive.html +fn setup_laser(mut commands: Commands) { + + // laser works on non-exclusive system (like the normal setup()), _but_ world + // is needed in the laser callback + + // Initialise the state that we want to live on the laser thread and spawn the stream. + let laser_model = LaserModel::new(); + let _laser_api = laser::Api::new(); + // let detected_dacs = _laser_api.detect_dacs(DacVariant::DacVariantHelios); + + // while let Ok(res) = detected_dacs { + // if let laser::DetectDacs::Helios { previous_dac } = res { + // info!("DACS: {:?}", previous_dac); + // } + // } + + let laser_stream = _laser_api + .new_frame_stream(laser_model, laser_frame_producer) + // .detected_dac(dac) + .build() + .unwrap(); + + + let api = LaserApi { + _laser_api, + laser_stream, + // current_points: Vec::new(), + }; + commands.spawn((api, LaserTimer { + // create the non-repeating fuse timer + timer: Timer::new(Duration::from_millis(1000), TimerMode::Repeating), + })); +} + +fn setup(mut commands: Commands) { + // Spawn a camera for our main window + // TODO: look into https://docs.rs/visioncortex/latest/visioncortex/struct.PerspectiveTransform.html + + commands.spawn(render::NannouCamera); + +} + + +fn text2points_with_color(position_and_intensity: PositionAndIntensity, color: Rgb) -> laser::Point{ + let used_color = color.map(|v| v * position_and_intensity[2]); + let p = [position_and_intensity[0], -position_and_intensity[1]]; + laser::Point::new(p, color) +} + +fn text2points(position_and_intensity: PositionAndIntensity) -> laser::Point{ + let color = match position_and_intensity[2] { + 1.0 => [1.0; 3], + 0.0 => [0.0; 3], + _ => [1.0; 3] // TODO add provided color + }; + let p = [position_and_intensity[0], -position_and_intensity[1]]; + laser::Point::new(p, color) +} + +// impl Into for [[f32;3]; 3] { +// fn from(m) -> Self{ + +// } +// } + +const LASER_H: Mat3 = python_cv_h_into_mat3(TMP_PYTHON_LASER_H); +// const LASER_H: Mat3 = python_cv_h_into_mat3(TMP_PYTHON_LASER_H_FOR_NANNOU); + +fn laser_frame_producer(model: &mut LaserModel, frame: &mut laser::Frame){ + + // let dt = model.t.elapsed().as_millis(); + // let use_second = (dt % 1000) > 500; + + // let positions = match use_second { + // true => trap::shapes::YOUR_FUTURE, + // false => trap::shapes::ARE_YOU_SURE, + // }; + + let points = model.current_points.clone(); + + let mut new_points = Vec::new(); + for point in points.into_iter() { + let p = point.position; + 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; // when using TMP_PYTHON_LASER_H + + let new_point = laser::Point { + position: [new_position[0]/s, new_position[1]/s], + .. point + }; + new_points.push(new_point); + } + + info!("Points {}", new_points.len()); + // println!("{:?}", new_points); + frame.add_lines(new_points); +} + + +fn get_laser_lines(use_second: bool) -> Vec{ + let positions = match use_second { + true => trap_rust::trap::shapes::YOUR_FUTURE, + false => trap_rust::trap::shapes::ARE_YOU_SURE, + }; + let points = positions.iter().cloned().map(text2points).collect(); + return points + +} + + + + + +fn exit_system(keys: Res>, mut exit: EventWriter) { + if keys.just_pressed(KeyCode::KeyQ) { + info!("Sending exit command"); + exit.send(AppExit::Success); + } +} + +fn update( + // mut commands: Commands, + // keys: Res>, + draws: Query<&Draw>, + mut lasers: Query<(&mut LaserApi, &mut LaserTimer)>, + tracks: Query<(&Track, &SpawnedTime)>, + time: Res