Merge pull request #207 from crono2250/fix/spacemouse-axis-mapping-198

Fix SpaceMouse axis mapping and apply 3DxWare settings

Cargo.lock

@@ -3297,6 +3297,7 @@ dependencies = [
  "embed-resource",
  "hidapi",
  "log",
+ "quick-xml 0.38.4",
  "rfd",
  "serde_json",
  "solvecraft-engine",

apps/solvecraft/Cargo.toml

@@ -21,6 +21,7 @@ rfd = { workspace = true }
 
 [target.'cfg(target_os = "windows")'.dependencies]
 hidapi = { version = "2.6.7", default-features = false, features = ["windows-native"] }
+quick-xml = { workspace = true }
 
 [target.'cfg(all(unix, not(target_os = "macos")))'.dependencies]
 winit = { version = "0.30", default-features = false, features = ["x11"] }

apps/solvecraft/src/space_mouse.rs

@@ -6,11 +6,14 @@ use std::time::{Duration, Instant};
 
 use egui::Context;
 use hidapi::{DeviceInfo, HidApi};
+use quick_xml::Reader;
+use quick_xml::events::Event;
 use solvecraft_engine::render::Camera;
 use solvecraft_ui_egui::SolveApp;
 
 const STALE_AFTER: Duration = Duration::from_millis(150);
 const RETRY_AFTER: Duration = Duration::from_secs(2);
+const MAX_PROFILE_BYTES: u64 = 1024 * 1024;
 
 #[derive(Clone, Copy, Debug, Default, PartialEq)]
 struct Axes {
@@ -18,6 +21,172 @@ struct Axes {
     rotation: [i16; 3],
 }
 
+#[derive(Clone, Copy, Debug, PartialEq)]
+struct AxisRule {
+    output: usize,
+    enabled: bool,
+    reversed: bool,
+    scale: f64,
+}
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+struct DriverMapping {
+    axes: [AxisRule; 6],
+    overall_scale: f64,
+}
+
+impl Default for DriverMapping {
+    fn default() -> Self {
+        Self { axes: std::array::from_fn(|output| AxisRule { output, enabled: true, reversed: false, scale: 1.0 }), overall_scale: 1.0 }
+    }
+}
+
+#[derive(Clone, Copy, Debug, Default, PartialEq)]
+struct Motion {
+    translation: [f64; 3],
+    rotation: [f64; 3],
+}
+
+impl DriverMapping {
+    fn apply(self, axes: Axes) -> Motion {
+        let raw = [
+            normalized(axes.translation[0]),
+            normalized(axes.translation[1]),
+            normalized(axes.translation[2]),
+            normalized(axes.rotation[0]),
+            normalized(axes.rotation[1]),
+            normalized(axes.rotation[2]),
+        ];
+        let mut logical = [0.0; 6];
+        for (input, rule) in self.axes.iter().enumerate() {
+            if rule.enabled {
+                let sign = if rule.reversed { -1.0 } else { 1.0 };
+                logical[rule.output] = raw[input] * sign * rule.scale * self.overall_scale;
+            }
+        }
+        Motion { translation: [logical[0], logical[1], logical[2]], rotation: [logical[3], logical[4], logical[5]] }
+    }
+}
+
+#[derive(Clone, Copy, Debug, Default)]
+struct ProfileAxis {
+    input: Option<usize>,
+    output: Option<usize>,
+    enabled: bool,
+    reversed: bool,
+    scale: f64,
+}
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+enum ProfilePart {
+    Input,
+    Output,
+}
+
+fn axis_index(name: &str) -> Option<usize> {
+    match name.trim() {
+        "HIDMultiAxis_X" => Some(0),
+        "HIDMultiAxis_Y" => Some(1),
+        "HIDMultiAxis_Z" => Some(2),
+        "HIDMultiAxis_Rx" => Some(3),
+        "HIDMultiAxis_Ry" => Some(4),
+        "HIDMultiAxis_Rz" => Some(5),
+        _ => None,
+    }
+}
+
+/// 3DxWare stores the active KMJ application's axis selection, swapping, inversion and speed in
+/// `%APPDATA%/3Dconnexion/3DxWare/Cfg/<app>-KMJ.xml`. Direct HID reads bypass the driver, so apply
+/// that profile explicitly when it exists. Unknown or incomplete fields retain the raw HID axis.
+fn parse_driver_mapping(xml: &[u8]) -> Option<DriverMapping> {
+    let mut reader = Reader::from_reader(xml);
+    reader.config_mut().trim_text(true);
+    let mut buf = Vec::new();
+    let mut mapping = DriverMapping::default();
+    let mut axis: Option<ProfileAxis> = None;
+    let mut part = None;
+    let mut field = Vec::new();
+    let mut found = false;
+    loop {
+        match reader.read_event_into(&mut buf).ok()? {
+            Event::Start(start) => match start.local_name().as_ref() {
+                b"Axis" => axis = Some(ProfileAxis { enabled: true, scale: 1.0, ..Default::default() }),
+                b"Input" => part = Some(ProfilePart::Input),
+                b"Output" => part = Some(ProfilePart::Output),
+                name @ (b"ActionID" | b"Enabled" | b"Reversed" | b"Scale" | b"OverallScale") => field = name.to_vec(),
+                _ => {}
+            },
+            Event::Text(text) => {
+                let value = String::from_utf8_lossy(text.as_ref());
+                match field.as_slice() {
+                    b"ActionID" => {
+                        if let Some(current) = axis.as_mut() {
+                            match part {
+                                Some(ProfilePart::Input) => current.input = axis_index(&value),
+                                Some(ProfilePart::Output) => current.output = axis_index(&value),
+                                None => {}
+                            }
+                        }
+                    }
+                    b"Enabled" => {
+                        if let Some(current) = axis.as_mut() {
+                            current.enabled = value.trim().eq_ignore_ascii_case("true");
+                        }
+                    }
+                    b"Reversed" => {
+                        if part == Some(ProfilePart::Output)
+                            && let Some(current) = axis.as_mut()
+                        {
+                            current.reversed = value.trim().eq_ignore_ascii_case("true");
+                        }
+                    }
+                    b"Scale" => {
+                        if part == Some(ProfilePart::Output)
+                            && let Some(scale) = value.trim().parse::<f64>().ok().filter(|scale| scale.is_finite())
+                            && let Some(current) = axis.as_mut()
+                        {
+                            current.scale = scale.clamp(0.0, 10.0);
+                        }
+                    }
+                    b"OverallScale" => {
+                        if let Some(scale) = value.trim().parse::<f64>().ok().filter(|scale| scale.is_finite()) {
+                            mapping.overall_scale = scale.clamp(0.0, 10.0);
+                        }
+                    }
+                    _ => {}
+                }
+            }
+            Event::End(end) => match end.local_name().as_ref() {
+                b"Axis" => {
+                    if let Some(current) = axis.take()
+                        && let (Some(input), Some(output)) = (current.input, current.output)
+                    {
+                        mapping.axes[input] = AxisRule { output, enabled: current.enabled, reversed: current.reversed, scale: current.scale };
+                        found = true;
+                    }
+                }
+                b"Input" | b"Output" => part = None,
+                b"ActionID" | b"Enabled" | b"Reversed" | b"Scale" | b"OverallScale" => field.clear(),
+                _ => {}
+            },
+            Event::Eof => break,
+            _ => {}
+        }
+        buf.clear();
+    }
+    found.then_some(mapping)
+}
+
+fn load_driver_mapping() -> Option<DriverMapping> {
+    let profile = std::path::PathBuf::from(std::env::var_os("APPDATA")?).join("3Dconnexion").join("3DxWare").join("Cfg").join("solvecraft-KMJ.xml");
+    if std::fs::metadata(&profile).ok()?.len() > MAX_PROFILE_BYTES {
+        log::warn!("3D mouse: ignoring oversized 3DxWare profile {}", profile.display());
+        return None;
+    }
+    let xml = std::fs::read(&profile).ok()?;
+    parse_driver_mapping(&xml)
+}
+
 #[derive(Clone, Copy)]
 struct Reading {
     axes: Axes,
@@ -107,6 +276,8 @@ fn reader(ctx: Context, shared: Arc<Mutex<Reading>>) {
 pub struct SpaceMouse {
     shared: Arc<Mutex<Reading>>,
     last_frame: Instant,
+    driver_mapping: DriverMapping,
+    mapping_checked_at: Instant,
 }
 
 impl SpaceMouse {
@@ -118,13 +289,21 @@ impl SpaceMouse {
         if let Err(error) = std::thread::Builder::new().name("solvecraft-3d-mouse".into()).spawn(move || reader(worker_ctx, worker_state)) {
             log::warn!("3D mouse: reader could not start: {error}");
         }
-        Self { shared, last_frame: now }
+        Self { shared, last_frame: now, driver_mapping: load_driver_mapping().unwrap_or_default(), mapping_checked_at: now }
     }
 
     pub fn tick(&mut self, app: &mut SolveApp, ctx: &Context) {
         let now = Instant::now();
         let seconds = now.duration_since(self.last_frame).as_secs_f64().min(0.05);
         self.last_frame = now;
+        if now.duration_since(self.mapping_checked_at) >= RETRY_AFTER {
+            let updated = load_driver_mapping().unwrap_or_default();
+            if updated != self.driver_mapping {
+                log::info!("3D mouse: applying updated 3DxWare axis profile");
+                self.driver_mapping = updated;
+            }
+            self.mapping_checked_at = now;
+        }
         if !app.preferences.space_mouse_enabled || app.home.open {
             return;
         }
@@ -141,7 +320,14 @@ impl SpaceMouse {
         }
         app.begin_external_navigation();
         let height = app.viewport.rect.map_or(600.0, |rect| f64::from(rect.height().max(1.0)));
-        navigate(&mut app.cam, axes, seconds * speed.clamp(0.1, 5.0), height, app.session.active_sketch.is_some());
+        navigate(
+            &mut app.cam,
+            self.driver_mapping.apply(axes),
+            seconds * speed.clamp(0.1, 5.0),
+            height,
+            app.session.active_sketch.is_some(),
+            app.preferences.zoom_reverse,
+        );
         ctx.request_repaint_after(Duration::from_millis(16));
     }
 }
@@ -153,15 +339,21 @@ fn normalized(value: i16) -> f64 {
     if v.abs() <= 20.0 { 0.0 } else { v.signum() * ((v.abs() - 20.0) / 330.0).min(1.0) }
 }
 
-fn navigate(cam: &mut Camera, axes: Axes, seconds: f64, height: f64, sketch: bool) {
+fn navigate(cam: &mut Camera, motion: Motion, seconds: f64, height: f64, sketch: bool, reverse_zoom: bool) {
     if !(seconds > 0.0) || !seconds.is_finite() {
         return;
     }
-    let [x, y, z] = axes.translation.map(normalized);
-    let [rx, ry, rz] = axes.rotation.map(normalized);
-    cam.pan(x * height * 0.6 * seconds, -y * height * 0.6 * seconds, height);
-    cam.zoom_at((-z * 1.5 * seconds).exp(), None);
+    let [x, y, z] = motion.translation;
+    let [rx, ry, rz] = motion.rotation;
+    // Logical 3DxWare coordinates are X = left/right, Y = zoom and Z = up/down. The user's
+    // profile above can swap the physical forward/back and up/down axes before they reach here.
+    cam.pan(x * height * 0.6 * seconds, z * height * 0.6 * seconds, height);
+    let zoom_sign = if reverse_zoom { -1.0 } else { 1.0 };
+    cam.zoom_at((-y * zoom_sign * 1.5 * seconds).exp(), None);
     if !sketch {
+        // These are logical 3DxWare coordinates after the profile mapping above: Rx is Tilt,
+        // Ry is Roll and Rz is Spin. The KMJ profile swaps physical Ry/Rz for the selected
+        // navigation mode, so swapping them again here would make Roll behave like Spin.
         cam.orbit(ry * 250.0 * seconds, rx * 250.0 * seconds);
         cam.roll_by(rz * 2.0 * seconds);
     }
@@ -171,6 +363,10 @@ fn navigate(cam: &mut Camera, axes: Axes, seconds: f64, height: f64, sketch: boo
 mod tests {
     use super::*;
 
+    fn raw(axes: Axes) -> Motion {
+        DriverMapping::default().apply(axes)
+    }
+
     #[test]
     fn decodes_separate_reports_without_stale_motion_or_padding_interference() {
         let now = Instant::now();
@@ -192,15 +388,102 @@ mod tests {
     fn six_axes_move_the_camera_and_sketches_keep_their_orientation() {
         let mut cam = Camera::default();
         let before = cam;
-        navigate(&mut cam, Axes { translation: [350, 350, 350], rotation: [350, 350, 350] }, 0.02, 600.0, false);
+        navigate(&mut cam, raw(Axes { translation: [350, 350, 350], rotation: [350, 350, 350] }), 0.02, 600.0, false, false);
         assert_ne!(cam.target, before.target);
         assert!(cam.distance < before.distance);
         assert_ne!(cam.yaw, before.yaw);
         assert_ne!(cam.pitch, before.pitch);
         assert_ne!(cam.roll, before.roll);
         let orientation = (cam.yaw, cam.pitch, cam.roll);
-        navigate(&mut cam, Axes { translation: [350, 0, 0], rotation: [350, 350, 350] }, 0.02, 600.0, true);
+        navigate(&mut cam, raw(Axes { translation: [350, 0, 0], rotation: [350, 350, 350] }), 0.02, 600.0, true, false);
         assert_eq!((cam.yaw, cam.pitch, cam.roll), orientation);
         assert!(cam.is_valid());
     }
+
+    #[test]
+    fn individual_axes_have_physical_spacemouse_meaning() {
+        let before = Camera::default();
+
+        let mut slide = before;
+        navigate(&mut slide, raw(Axes { translation: [350, 0, 0], rotation: [0; 3] }), 0.02, 600.0, false, false);
+        assert_ne!(slide.target, before.target);
+        assert_eq!(slide.distance, before.distance);
+
+        let mut push = before;
+        navigate(&mut push, raw(Axes { translation: [0, 350, 0], rotation: [0; 3] }), 0.02, 600.0, false, false);
+        assert_eq!(push.target, before.target);
+        assert!(push.distance < before.distance);
+
+        let mut lift = before;
+        navigate(&mut lift, raw(Axes { translation: [0, 0, 350], rotation: [0; 3] }), 0.02, 600.0, false, false);
+        assert_ne!(lift.target, before.target);
+        assert_eq!(lift.distance, before.distance);
+
+        let mut tilt = before;
+        navigate(&mut tilt, raw(Axes { translation: [0; 3], rotation: [350, 0, 0] }), 0.02, 600.0, false, false);
+        assert_eq!((tilt.yaw, tilt.roll), (before.yaw, before.roll));
+        assert_ne!(tilt.pitch, before.pitch);
+
+        let mut roll = before;
+        navigate(&mut roll, raw(Axes { translation: [0; 3], rotation: [0, 350, 0] }), 0.02, 600.0, false, false);
+        assert_eq!((roll.pitch, roll.roll), (before.pitch, before.roll));
+        assert_ne!(roll.yaw, before.yaw);
+
+        let mut twist = before;
+        navigate(&mut twist, raw(Axes { translation: [0; 3], rotation: [0, 0, 350] }), 0.02, 600.0, false, false);
+        assert_eq!((twist.yaw, twist.pitch), (before.yaw, before.pitch));
+        assert_ne!(twist.roll, before.roll);
+    }
+
+    #[test]
+    fn reverse_zoom_setting_applies_to_spacemouse() {
+        let before = Camera::default();
+        let motion = raw(Axes { translation: [0, 350, 0], rotation: [0; 3] });
+        let mut normal = before;
+        navigate(&mut normal, motion, 0.02, 600.0, false, false);
+        let mut reversed = before;
+        navigate(&mut reversed, motion, 0.02, 600.0, false, true);
+        assert!(normal.distance < before.distance);
+        assert!(reversed.distance > before.distance);
+    }
+
+    #[test]
+    fn applies_3dxware_axis_swaps_and_reversals() {
+        let profile = br#"
+            <AppCfg><Devices><Device><AxisBank>
+              <Axis><Enabled>true</Enabled><Input><ActionID>HIDMultiAxis_X</ActionID></Input><Output><ActionID>HIDMultiAxis_X</ActionID><Reversed>false</Reversed></Output></Axis>
+              <Axis><Enabled>true</Enabled><Input><ActionID>HIDMultiAxis_Y</ActionID></Input><Output><ActionID>HIDMultiAxis_Z</ActionID><Reversed>false</Reversed></Output></Axis>
+              <Axis><Enabled>true</Enabled><Input><ActionID>HIDMultiAxis_Z</ActionID></Input><Output><ActionID>HIDMultiAxis_Y</ActionID><Reversed>true</Reversed></Output></Axis>
+              <Axis><Enabled>true</Enabled><Input><ActionID>HIDMultiAxis_Rx</ActionID></Input><Output><ActionID>HIDMultiAxis_Rx</ActionID><Reversed>false</Reversed></Output></Axis>
+              <Axis><Enabled>true</Enabled><Input><ActionID>HIDMultiAxis_Ry</ActionID></Input><Output><ActionID>HIDMultiAxis_Rz</ActionID><Reversed>false</Reversed></Output></Axis>
+              <Axis><Enabled>true</Enabled><Input><ActionID>HIDMultiAxis_Rz</ActionID></Input><Output><ActionID>HIDMultiAxis_Ry</ActionID><Reversed>true</Reversed></Output></Axis>
+            </AxisBank></Device></Devices></AppCfg>
+        "#;
+        let mapping = parse_driver_mapping(profile).expect("valid 3DxWare profile");
+        let motion = mapping.apply(Axes { translation: [350, 0, 350], rotation: [350, 0, 350] });
+        assert_eq!(motion.translation, [1.0, -1.0, 0.0]);
+        assert_eq!(motion.rotation, [1.0, -1.0, 0.0]);
+    }
+
+    #[test]
+    fn mapped_roll_and_spin_reach_distinct_camera_rotations() {
+        let profile = br#"
+            <AppCfg><Devices><Device><AxisBank>
+              <Axis><Enabled>true</Enabled><Input><ActionID>HIDMultiAxis_Ry</ActionID></Input><Output><ActionID>HIDMultiAxis_Rz</ActionID><Reversed>false</Reversed></Output></Axis>
+              <Axis><Enabled>true</Enabled><Input><ActionID>HIDMultiAxis_Rz</ActionID></Input><Output><ActionID>HIDMultiAxis_Ry</ActionID><Reversed>true</Reversed></Output></Axis>
+            </AxisBank></Device></Devices></AppCfg>
+        "#;
+        let mapping = parse_driver_mapping(profile).expect("valid 3DxWare profile");
+        let before = Camera::default();
+
+        let mut physical_roll = before;
+        navigate(&mut physical_roll, mapping.apply(Axes { translation: [0; 3], rotation: [0, 350, 0] }), 0.02, 600.0, false, false);
+        assert_eq!((physical_roll.yaw, physical_roll.pitch), (before.yaw, before.pitch));
+        assert_ne!(physical_roll.roll, before.roll);
+
+        let mut physical_spin = before;
+        navigate(&mut physical_spin, mapping.apply(Axes { translation: [0; 3], rotation: [0, 0, 350] }), 0.02, 600.0, false, false);
+        assert_ne!(physical_spin.yaw, before.yaw);
+        assert_eq!((physical_spin.pitch, physical_spin.roll), (before.pitch, before.roll));
+    }
 }