diff --git a/src/main.rs b/src/main.rs index 9b887c9..349c45a 100644 --- a/src/main.rs +++ b/src/main.rs @@ -52,6 +52,7 @@ use nodes::audio::AudioNodePlugin; use nodes::drawable::lines::LinesNodePlugin; use nodes::drawable::model::ModelNodePlugin; use nodes::drawable::text::TextNodePlugin; +use nodes::fields::FieldDebugGizmoPlugin; use nodes::spatial::SpatialNodePlugin; use objects::hmd::HmdPlugin; use objects::input::mouse_pointer::FlatscreenInputPlugin; @@ -400,7 +401,7 @@ fn bevy_loop( // object plugins app.add_plugins((PlaySpacePlugin, HandPlugin, ControllerPlugin, HmdPlugin)); // feature plugins - app.add_plugins((WaylandPlugin, TrackingOffsetPlugin)); + app.add_plugins((WaylandPlugin, TrackingOffsetPlugin, FieldDebugGizmoPlugin)); app.add_systems(PostStartup, move || { ready_notifier.notify_waiters(); }); diff --git a/src/nodes/drawable/lines.rs b/src/nodes/drawable/lines.rs index 9c7d7ce..f985f86 100644 --- a/src/nodes/drawable/lines.rs +++ b/src/nodes/drawable/lines.rs @@ -107,7 +107,10 @@ fn build_line_mesh( ) }); let quat = match (last_quat, next_quat) { - (None, None) => unreachable!(), + (None, None) => { + error!("no previous or next point in line"); + break; + } (None, Some(next)) => next, (Some(last), None) => last, (Some(last), Some(next)) => last.lerp(next, 0.5), @@ -148,11 +151,36 @@ fn build_line_mesh( PrimitiveTopology::TriangleList, RenderAssetUsages::RENDER_WORLD, ); + if vertex_colors.iter().flatten().any(|v| !v.is_finite()) { + panic!( + "vertex colors contains non finite float: {:#?}", + vertex_colors + ); + } + if vertex_normals.iter().any(|v| !v.is_finite()) { + panic!( + "normals contains non finite dir: {:#?}", + vertex_normals + ); + } + if vertex_normals.iter().any(|v| !v.is_normalized()) { + panic!( + "normals contains non normalized dir: {:#?}", + vertex_normals + ); + } + if vertex_positions.iter().any(|v| !v.is_finite()) { + panic!( + "vertex positions contains non finite pos: {:#?}", + vertex_positions + ); + } mesh.insert_indices(Indices::U32(vertex_indices)); mesh.insert_attribute(Mesh::ATTRIBUTE_COLOR, vertex_colors); mesh.insert_attribute(Mesh::ATTRIBUTE_NORMAL, vertex_normals); mesh.insert_attribute(Mesh::ATTRIBUTE_POSITION, vertex_positions.clone()); if let Some(aabb) = mesh.compute_aabb() { + info!(?aabb); *lines.bounds.lock() = aabb; } diff --git a/src/nodes/fields.rs b/src/nodes/fields.rs index dbef5cc..3c973d0 100644 --- a/src/nodes/fields.rs +++ b/src/nodes/fields.rs @@ -5,17 +5,28 @@ use super::spatial::{ Spatial, }; use super::{Aspect, AspectIdentifier, Node}; +use crate::DbusConnection; use crate::core::client::Client; use crate::core::error::Result; +use crate::core::registry::Registry; use crate::nodes::spatial::SPATIAL_ASPECT_ALIAS_INFO; use crate::nodes::spatial::SPATIAL_REF_ASPECT_ALIAS_INFO; use crate::nodes::spatial::Transform; +use bevy::app::{Plugin, Update}; +use bevy::color::Color; +use bevy::ecs::resource::Resource; +use bevy::ecs::schedule::IntoScheduleConfigs; +use bevy::ecs::system::Res; +use bevy::gizmos::gizmos::Gizmos; +use bevy::gizmos::primitives::dim3::GizmoPrimitive3d; +use bevy::math::primitives::{Cylinder, Torus}; use color_eyre::eyre::OptionExt; use glam::{Vec3, Vec3A, Vec3Swizzles, vec2, vec3, vec3a}; use parking_lot::Mutex; use rustc_hash::FxHashMap; use stardust_xr::values::Vector3; use std::sync::{Arc, LazyLock}; +use zbus::interface; // TODO: get SDFs working properly with non-uniform scale and so on, output distance relative to the spatial it's compared against @@ -32,6 +43,90 @@ pub static FIELD_ALIAS_INFO: LazyLock = LazyLock::new(|| AliasInfo { ..Default::default() }); +pub struct FieldDebugGizmoPlugin; +impl Plugin for FieldDebugGizmoPlugin { + fn build(&self, app: &mut bevy::app::App) { + let (tx, rx) = tokio::sync::watch::channel(false); + let conn = app.world().resource::().0.clone(); + tokio::spawn(async move { + _ = conn + .object_server() + .at("/org/stardustxr/Server", FieldDebugGizmos { state: tx }) + .await; + }); + app.insert_resource(FieldDebugGizmosEnabled(rx)); + app.add_systems( + Update, + draw_field_gizmos.run_if(|res: Res| *res.0.borrow()), + ); + } +} + +#[derive(Resource)] +struct FieldDebugGizmosEnabled(tokio::sync::watch::Receiver); + +fn draw_field_gizmos(mut gizmos: Gizmos) { + FIELD_REGISTRY_DEBUG_GIZMOS + .get_valid_contents() + .iter() + .for_each(|f| { + let transform = + bevy::transform::components::Transform::from_matrix(f.spatial.global_transform()); + let color = Color::srgb_u8(0x04, 0xFD, 0x4C); + match f.shape.lock().clone() { + Shape::Box(size) => gizmos.cuboid(transform.with_scale(size.into()), color), + Shape::Cylinder(CylinderShape { length, radius }) => { + gizmos.primitive_3d( + &Cylinder { + radius, + half_height: length * 0.5, + }, + transform.to_isometry(), + color, + ); + } + Shape::Sphere(radius) => { + gizmos.sphere(transform.to_isometry(), radius, color); + } + Shape::Torus(TorusShape { radius_a, radius_b }) => { + let minor_radius; + let major_radius; + if radius_a >= radius_b { + major_radius = radius_a; + minor_radius = radius_b; + } else { + major_radius = radius_b; + minor_radius = radius_a; + } + gizmos.primitive_3d( + &Torus { + minor_radius, + major_radius, + }, + transform.to_isometry(), + color, + ); + } + } + }); +} + +struct FieldDebugGizmos { + state: tokio::sync::watch::Sender, +} + +#[interface(name = "org.stardustxr.debug.FieldDebugGizmos")] +impl FieldDebugGizmos { + fn enable(&mut self) { + _ = self.state.send(true); + } + fn disable(&mut self) { + _ = self.state.send(false); + } +} + +static FIELD_REGISTRY_DEBUG_GIZMOS: Registry = Registry::new(); + stardust_xr_server_codegen::codegen_field_protocol!(); lazy_static::lazy_static! { @@ -145,10 +240,16 @@ impl Field { shape: Mutex::new(shape), }; let field = node.add_aspect(field); + FIELD_REGISTRY_DEBUG_GIZMOS.add_raw(&field); node.add_aspect(FieldRef); Ok(field) } } +impl Drop for Field { + fn drop(&mut self) { + FIELD_REGISTRY_DEBUG_GIZMOS.remove(self); + } +} impl AspectIdentifier for Field { impl_aspect_for_field_aspect_id! {} }