Skip to main content

webizen_studio/render/
gizmo_3d.rs

1//! Zero-Heap 3D PGA Motor Manipulation Gizmos (`gizmo_3d.rs`)
2//!
3//! Implements Epic 9.3a of the QualiaDB Superior Game Engine architecture:
4//! - Ray-pickable 3D manipulation gizmo ([`MotorGizmo3D`]) supporting [`GizmoMode::Translate`],
5//!   [`GizmoMode::Rotate`], [`GizmoMode::Scale`], and [`GizmoMode::MotorScrew`] (simultaneous
6//!   rotation + translation along a Chasles screw axis in Projective Geometric Algebra).
7//! - Tests mouse picking rays against X/Y/Z axis arrows, XY/YZ/XZ plane handles, and X/Y/Z
8//!   rotation rings in **zero heap**.
9//! - Outputs exact PGA [`Motor`] deltas (`delta_motor: Motor`, `delta_scale: [f32; 3]`).
10
11/// 3D Projective Geometric Algebra (PGA) Motor (`Motor { r, d }`) representing rigid body
12/// transformations (rotations, translations, and general Chasles screw motions) in $\mathbf{P}(\mathbb{R}^*_{3,0,1})$.
13#[repr(C)]
14#[derive(Debug, Clone, Copy, PartialEq)]
15pub struct Motor {
16    /// Rotor component `[s, e12, e13, e23]`.
17    pub r: [f32; 4],
18    /// Dual translation/screw component `[e0123, e03, e02, e01]`.
19    pub d: [f32; 4],
20}
21
22impl Default for Motor {
23    fn default() -> Self {
24        Self::identity()
25    }
26}
27
28#[cfg(not(target_arch = "wasm32"))]
29impl From<qualia_core_db::render::pga::Motor> for Motor {
30    #[inline]
31    fn from(m: qualia_core_db::render::pga::Motor) -> Self {
32        Self { r: m.r, d: m.d }
33    }
34}
35
36#[cfg(not(target_arch = "wasm32"))]
37impl From<Motor> for qualia_core_db::render::pga::Motor {
38    #[inline]
39    fn from(m: Motor) -> Self {
40        Self { r: m.r, d: m.d }
41    }
42}
43
44impl Motor {
45    /// Identity PGA motor.
46    #[inline]
47    pub const fn identity() -> Self {
48        Self {
49            r: [1.0, 0.0, 0.0, 0.0],
50            d: [0.0, 0.0, 0.0, 0.0],
51        }
52    }
53
54    /// Pure translation motor along `v = [dx, dy, dz]`.
55    #[inline]
56    pub fn from_translation(v: [f32; 3]) -> Self {
57        Self {
58            r: [1.0, 0.0, 0.0, 0.0],
59            d: quat_to_blade4([0.0, v[0] * 0.5, v[1] * 0.5, v[2] * 0.5]),
60        }
61    }
62
63    /// Pure rotation motor around `axis` by `angle_rad` radians.
64    #[inline]
65    pub fn from_axis_angle(axis: [f32; 3], angle_rad: f32) -> Self {
66        let a = normalize3(axis, [0.0, 1.0, 0.0]);
67        let half = angle_rad * 0.5;
68        let (s, c) = half.sin_cos();
69        Self {
70            r: [c, -s * a[2], s * a[1], -s * a[0]],
71            d: [0.0, 0.0, 0.0, 0.0],
72        }
73    }
74
75    /// Chasles screw motor combining rotation `angle_rad` around `axis` and linear translation
76    /// `slide_m` along `axis`.
77    #[inline]
78    pub fn from_screw(axis: [f32; 3], angle_rad: f32, slide_m: f32) -> Self {
79        let a = normalize3(axis, [0.0, 1.0, 0.0]);
80        let rot = Self::from_axis_angle(a, angle_rad);
81        let trans = Self::from_translation([a[0] * slide_m, a[1] * slide_m, a[2] * slide_m]);
82        trans.motor_mul(rot)
83    }
84
85    /// Dual-quaternion geometric product $M_a M_b$.
86    #[inline]
87    pub fn motor_mul(self, rhs: Self) -> Self {
88        let qr1 = blade4_to_quat(self.r);
89        let qd1 = blade4_to_quat(self.d);
90        let qr2 = blade4_to_quat(rhs.r);
91        let qd2 = blade4_to_quat(rhs.d);
92        let qr3 = quat_mul(qr1, qr2);
93        let q1d2 = quat_mul(qr1, qd2);
94        let qd12 = quat_mul(qd1, qr2);
95        let qd3 = [
96            q1d2[0] + qd12[0],
97            q1d2[1] + qd12[1],
98            q1d2[2] + qd12[2],
99            q1d2[3] + qd12[3],
100        ];
101        Self {
102            r: quat_to_blade4(qr3),
103            d: quat_to_blade4(qd3),
104        }
105    }
106
107    /// PGA reversion $\widetilde{M}$.
108    #[inline]
109    pub const fn motor_reverse(self) -> Self {
110        Self {
111            r: [self.r[0], -self.r[1], -self.r[2], -self.r[3]],
112            d: [self.d[0], -self.d[1], -self.d[2], -self.d[3]],
113        }
114    }
115
116    /// Applies the PGA sandwich product $P' = M P \widetilde{M}$ to a 3D Euclidean point.
117    #[inline]
118    pub fn sandwich_point(self, p: [f32; 3]) -> [f32; 3] {
119        let qr = blade4_to_quat(self.r);
120        let qd = blade4_to_quat(self.d);
121        let qr_conj = [qr[0], -qr[1], -qr[2], -qr[3]];
122        let pq = [0.0, p[0], p[1], p[2]];
123        let p_rot = quat_mul(quat_mul(qr, pq), qr_conj);
124        let tq = quat_mul(qd, qr_conj);
125        [
126            p_rot[1] + 2.0 * tq[1],
127            p_rot[2] + 2.0 * tq[2],
128            p_rot[3] + 2.0 * tq[3],
129        ]
130    }
131}
132
133/// Active manipulation mode for [`MotorGizmo3D`].
134#[repr(u8)]
135#[derive(Debug, Clone, Copy, PartialEq, Eq)]
136pub enum GizmoMode {
137    Translate = 0,
138    Rotate = 1,
139    Scale = 2,
140    MotorScrew = 3,
141}
142
143impl Default for GizmoMode {
144    fn default() -> Self {
145        Self::Translate
146    }
147}
148
149/// Pickable sub-handle of a [`MotorGizmo3D`].
150#[repr(u8)]
151#[derive(Debug, Clone, Copy, PartialEq, Eq)]
152pub enum GizmoHandle {
153    AxisX = 0,
154    AxisY = 1,
155    AxisZ = 2,
156    PlaneXY = 3,
157    PlaneYZ = 4,
158    PlaneXZ = 5,
159    RingX = 6,
160    RingY = 7,
161    RingZ = 8,
162    UniformCenter = 9,
163}
164
165impl GizmoHandle {
166    /// Returns the canonical local unit axis `[x, y, z]` associated with this handle.
167    #[inline]
168    pub const fn unit_axis(self) -> [f32; 3] {
169        match self {
170            Self::AxisX | Self::RingX | Self::PlaneYZ => [1.0, 0.0, 0.0],
171            Self::AxisY | Self::RingY | Self::PlaneXZ => [0.0, 1.0, 0.0],
172            Self::AxisZ | Self::RingZ | Self::PlaneXY => [0.0, 0.0, 1.0],
173            Self::UniformCenter => [1.0, 1.0, 1.0],
174        }
175    }
176}
177
178/// Result of a ray-pick test against [`MotorGizmo3D`].
179#[derive(Debug, Clone, Copy, PartialEq)]
180pub struct GizmoPickHit {
181    pub handle: GizmoHandle,
182    /// Ray parameter `t` along `ray_origin + t * ray_dir`.
183    pub ray_t: f32,
184    /// World-space closest hit point on the gizmo handle.
185    pub hit_point: [f32; 3],
186    /// Parameter along the axis `[0.0, axis_length]` or angle in radians on a rotation ring.
187    pub handle_param: f32,
188}
189
190/// Output delta produced by dragging an active [`MotorGizmo3D`] handle.
191#[derive(Debug, Clone, Copy, PartialEq)]
192pub struct GizmoDragDelta {
193    /// Exact incremental PGA [`Motor`] delta (`new_motor = delta_motor.motor_mul(old_motor)`).
194    pub delta_motor: Motor,
195    /// Multiplicative scale factor delta `[sx, sy, sz]` (`1.0` = unchanged).
196    pub delta_scale: [f32; 3],
197}
198
199impl Default for GizmoDragDelta {
200    fn default() -> Self {
201        Self {
202            delta_motor: Motor::identity(),
203            delta_scale: [1.0, 1.0, 1.0],
204        }
205    }
206}
207
208/// Zero-heap 3D ray-pickable PGA Motor manipulation gizmo.
209#[derive(Debug, Clone, Copy, PartialEq)]
210pub struct MotorGizmo3D {
211    /// Current world-space pose of the gizmo pivot.
212    pub pivot_motor: Motor,
213    /// Current manipulation mode.
214    pub mode: GizmoMode,
215    /// Length of the X/Y/Z axis arrows in world units.
216    pub axis_length: f32,
217    /// Radius of the X/Y/Z rotation rings in world units.
218    pub ring_radius: f32,
219    /// Picking tolerance tube radius in world units.
220    pub pick_tolerance: f32,
221    /// Currently hovered handle.
222    pub hovered_handle: Option<GizmoHandle>,
223    /// Currently dragged handle.
224    pub active_handle: Option<GizmoHandle>,
225}
226
227impl Default for MotorGizmo3D {
228    fn default() -> Self {
229        Self {
230            pivot_motor: Motor::identity(),
231            mode: GizmoMode::Translate,
232            axis_length: 1.5,
233            ring_radius: 1.25,
234            pick_tolerance: 0.14,
235            hovered_handle: None,
236            active_handle: None,
237        }
238    }
239}
240
241impl MotorGizmo3D {
242    /// Creates a gizmo at `pivot_motor` with the given `mode`.
243    #[inline]
244    pub const fn new(pivot_motor: Motor, mode: GizmoMode) -> Self {
245        Self {
246            pivot_motor,
247            mode,
248            axis_length: 1.5,
249            ring_radius: 1.25,
250            pick_tolerance: 0.14,
251            hovered_handle: None,
252            active_handle: None,
253        }
254    }
255
256    /// World-space origin of the gizmo pivot.
257    #[inline]
258    pub fn world_origin(&self) -> [f32; 3] {
259        self.pivot_motor.sandwich_point([0.0, 0.0, 0.0])
260    }
261
262    /// World-space unit direction for a local axis `[lx, ly, lz]`.
263    #[inline]
264    pub fn world_axis(&self, local_axis: [f32; 3]) -> [f32; 3] {
265        let o = self.world_origin();
266        let tip = self.pivot_motor.sandwich_point(local_axis);
267        normalize3(sub3(tip, o), local_axis)
268    }
269
270    /// Tests a world-space mouse picking ray `(ray_origin, ray_dir)` against the gizmo's
271    /// X/Y/Z axis arrows, XY/YZ/XZ plane quads, and X/Y/Z rotation rings in **zero heap**.
272    pub fn pick_ray(&self, ray_origin: [f32; 3], ray_dir: [f32; 3]) -> Option<GizmoPickHit> {
273        let dir = normalize3(ray_dir, [0.0, 0.0, -1.0]);
274        let origin = self.world_origin();
275        let axes = [
276            (
277                GizmoHandle::AxisX,
278                GizmoHandle::RingX,
279                self.world_axis([1.0, 0.0, 0.0]),
280            ),
281            (
282                GizmoHandle::AxisY,
283                GizmoHandle::RingY,
284                self.world_axis([0.0, 1.0, 0.0]),
285            ),
286            (
287                GizmoHandle::AxisZ,
288                GizmoHandle::RingZ,
289                self.world_axis([0.0, 0.0, 1.0]),
290            ),
291        ];
292
293        let mut best: Option<GizmoPickHit> = None;
294        let mut best_dist = self.pick_tolerance;
295
296        // 1. Test rotation rings when in Rotate or MotorScrew mode
297        if matches!(self.mode, GizmoMode::Rotate | GizmoMode::MotorScrew) {
298            for &(_, ring_handle, axis_dir) in &axes {
299                let denom = dot3(dir, axis_dir);
300                if denom.abs() > 1e-4 {
301                    let t = dot3(sub3(origin, ray_origin), axis_dir) / denom;
302                    if t > 0.0 {
303                        let p = add3(ray_origin, scale3(dir, t));
304                        let radial = sub3(p, origin);
305                        let r_len = length3(radial);
306                        let ring_err = (r_len - self.ring_radius).abs();
307                        if ring_err <= best_dist {
308                            best_dist = ring_err;
309                            let angle = radial[1].atan2(radial[0]);
310                            best = Some(GizmoPickHit {
311                                handle: ring_handle,
312                                ray_t: t,
313                                hit_point: p,
314                                handle_param: angle,
315                            });
316                        }
317                    }
318                }
319            }
320        }
321
322        // 2. Test X/Y/Z axis arrow segments (Translate, Scale, MotorScrew)
323        if matches!(
324            self.mode,
325            GizmoMode::Translate | GizmoMode::Scale | GizmoMode::MotorScrew
326        ) {
327            for &(axis_handle, _, axis_dir) in &axes {
328                let seg_end = add3(origin, scale3(axis_dir, self.axis_length));
329                let (ray_t, seg_s, dist, pt_on_seg) =
330                    closest_points_ray_segment(ray_origin, dir, origin, seg_end);
331                if ray_t > 0.0 && dist <= best_dist {
332                    best_dist = dist;
333                    best = Some(GizmoPickHit {
334                        handle: axis_handle,
335                        ray_t,
336                        hit_point: pt_on_seg,
337                        handle_param: seg_s * self.axis_length,
338                    });
339                }
340            }
341
342            // 3. Test XY, YZ, XZ planar translation quads `[0.25 .. 0.55] * axis_length`
343            let planes = [
344                (
345                    GizmoHandle::PlaneXY,
346                    axes[0].2,
347                    axes[1].2,
348                    axes[2].2,
349                ),
350                (
351                    GizmoHandle::PlaneYZ,
352                    axes[1].2,
353                    axes[2].2,
354                    axes[0].2,
355                ),
356                (
357                    GizmoHandle::PlaneXZ,
358                    axes[0].2,
359                    axes[2].2,
360                    axes[1].2,
361                ),
362            ];
363            let quad_min = self.axis_length * 0.22;
364            let quad_max = self.axis_length * 0.55;
365            for &(plane_handle, u_axis, v_axis, n_axis) in &planes {
366                let denom = dot3(dir, n_axis);
367                if denom.abs() > 1e-4 {
368                    let t = dot3(sub3(origin, ray_origin), n_axis) / denom;
369                    if t > 0.0 {
370                        let p = add3(ray_origin, scale3(dir, t));
371                        let rel = sub3(p, origin);
372                        let u = dot3(rel, u_axis);
373                        let v = dot3(rel, v_axis);
374                        if u >= quad_min && u <= quad_max && v >= quad_min && v <= quad_max {
375                            best = Some(GizmoPickHit {
376                                handle: plane_handle,
377                                ray_t: t,
378                                hit_point: p,
379                                handle_param: u + v,
380                            });
381                        }
382                    }
383                }
384            }
385        }
386
387        best
388    }
389
390    /// Computes the exact PGA [`Motor`] and scale delta between an initial mouse ray and
391    /// current mouse ray for `handle`.
392    pub fn compute_drag_delta(
393        &self,
394        handle: GizmoHandle,
395        start_ray_origin: [f32; 3],
396        start_ray_dir: [f32; 3],
397        cur_ray_origin: [f32; 3],
398        cur_ray_dir: [f32; 3],
399    ) -> GizmoDragDelta {
400        let s_dir = normalize3(start_ray_dir, [0.0, 0.0, -1.0]);
401        let c_dir = normalize3(cur_ray_dir, [0.0, 0.0, -1.0]);
402        let origin = self.world_origin();
403        let world_axis = self.world_axis(handle.unit_axis());
404
405        match (self.mode, handle) {
406            (
407                GizmoMode::Rotate,
408                GizmoHandle::RingX | GizmoHandle::RingY | GizmoHandle::RingZ | GizmoHandle::AxisX | GizmoHandle::AxisY | GizmoHandle::AxisZ,
409            ) => {
410                let p0 = intersect_ray_plane(start_ray_origin, s_dir, origin, world_axis);
411                let p1 = intersect_ray_plane(cur_ray_origin, c_dir, origin, world_axis);
412                let v0 = normalize3(sub3(p0, origin), [1.0, 0.0, 0.0]);
413                let v1 = normalize3(sub3(p1, origin), [1.0, 0.0, 0.0]);
414                let sin_a = dot3(cross3(v0, v1), world_axis);
415                let cos_a = dot3(v0, v1).clamp(-1.0, 1.0);
416                let angle = sin_a.atan2(cos_a);
417                // Rotate around pivot origin: T(origin) * R(axis, angle) * T(-origin)
418                let to_origin = Motor::from_translation([-origin[0], -origin[1], -origin[2]]);
419                let rot = Motor::from_axis_angle(world_axis, angle);
420                let back = Motor::from_translation(origin);
421                GizmoDragDelta {
422                    delta_motor: back.motor_mul(rot.motor_mul(to_origin)),
423                    delta_scale: [1.0, 1.0, 1.0],
424                }
425            }
426            (GizmoMode::Scale, GizmoHandle::AxisX | GizmoHandle::AxisY | GizmoHandle::AxisZ) => {
427                let s0 = project_ray_onto_axis(start_ray_origin, s_dir, origin, world_axis);
428                let s1 = project_ray_onto_axis(cur_ray_origin, c_dir, origin, world_axis);
429                let ratio = ((self.axis_length + (s1 - s0)) / self.axis_length.max(0.1))
430                    .clamp(0.05, 20.0);
431                let mut scale = [1.0, 1.0, 1.0];
432                match handle {
433                    GizmoHandle::AxisX => scale[0] = ratio,
434                    GizmoHandle::AxisY => scale[1] = ratio,
435                    GizmoHandle::AxisZ => scale[2] = ratio,
436                    _ => {}
437                }
438                GizmoDragDelta {
439                    delta_motor: Motor::identity(),
440                    delta_scale: scale,
441                }
442            }
443            (GizmoMode::MotorScrew, _) => {
444                let s0 = project_ray_onto_axis(start_ray_origin, s_dir, origin, world_axis);
445                let s1 = project_ray_onto_axis(cur_ray_origin, c_dir, origin, world_axis);
446                let slide = s1 - s0;
447                // Chasles screw motion: 1 radian per meter of axial slide
448                let angle = slide * 1.0;
449                let to_origin = Motor::from_translation([-origin[0], -origin[1], -origin[2]]);
450                let screw = Motor::from_screw(world_axis, angle, slide);
451                let back = Motor::from_translation(origin);
452                GizmoDragDelta {
453                    delta_motor: back.motor_mul(screw.motor_mul(to_origin)),
454                    delta_scale: [1.0, 1.0, 1.0],
455                }
456            }
457            (_, GizmoHandle::PlaneXY | GizmoHandle::PlaneYZ | GizmoHandle::PlaneXZ) => {
458                let p0 = intersect_ray_plane(start_ray_origin, s_dir, origin, world_axis);
459                let p1 = intersect_ray_plane(cur_ray_origin, c_dir, origin, world_axis);
460                let delta = sub3(p1, p0);
461                GizmoDragDelta {
462                    delta_motor: Motor::from_translation(delta),
463                    delta_scale: [1.0, 1.0, 1.0],
464                }
465            }
466            _ => {
467                // Linear translation along the picked axis
468                let s0 = project_ray_onto_axis(start_ray_origin, s_dir, origin, world_axis);
469                let s1 = project_ray_onto_axis(cur_ray_origin, c_dir, origin, world_axis);
470                let slide = s1 - s0;
471                let delta = scale3(world_axis, slide);
472                GizmoDragDelta {
473                    delta_motor: Motor::from_translation(delta),
474                    delta_scale: [1.0, 1.0, 1.0],
475                }
476            }
477        }
478    }
479}
480
481#[inline]
482fn blade4_to_quat(v: [f32; 4]) -> [f32; 4] {
483    [v[0], -v[3], -v[2], -v[1]]
484}
485
486#[inline]
487fn quat_to_blade4(q: [f32; 4]) -> [f32; 4] {
488    [q[0], -q[3], -q[2], -q[1]]
489}
490
491#[inline]
492fn quat_mul(a: [f32; 4], b: [f32; 4]) -> [f32; 4] {
493    [
494        a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3],
495        a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2],
496        a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1],
497        a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0],
498    ]
499}
500
501#[inline]
502fn add3(a: [f32; 3], b: [f32; 3]) -> [f32; 3] {
503    [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
504}
505
506#[inline]
507fn sub3(a: [f32; 3], b: [f32; 3]) -> [f32; 3] {
508    [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
509}
510
511#[inline]
512fn scale3(v: [f32; 3], s: f32) -> [f32; 3] {
513    [v[0] * s, v[1] * s, v[2] * s]
514}
515
516#[inline]
517fn dot3(a: [f32; 3], b: [f32; 3]) -> f32 {
518    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
519}
520
521#[inline]
522fn cross3(a: [f32; 3], b: [f32; 3]) -> [f32; 3] {
523    [
524        a[1] * b[2] - a[2] * b[1],
525        a[2] * b[0] - a[0] * b[2],
526        a[0] * b[1] - a[1] * b[0],
527    ]
528}
529
530#[inline]
531fn length3(v: [f32; 3]) -> f32 {
532    dot3(v, v).sqrt()
533}
534
535#[inline]
536fn normalize3(v: [f32; 3], fallback: [f32; 3]) -> [f32; 3] {
537    let l = length3(v);
538    if l > 1e-6 {
539        [v[0] / l, v[1] / l, v[2] / l]
540    } else {
541        fallback
542    }
543}
544
545/// Closest approach between ray `ro + t * rd` (`t >= 0`) and segment `a .. b` (`s in [0, 1]`).
546fn closest_points_ray_segment(
547    ro: [f32; 3],
548    rd: [f32; 3],
549    a: [f32; 3],
550    b: [f32; 3],
551) -> (f32, f32, f32, [f32; 3]) {
552    let u = rd;
553    let v = sub3(b, a);
554    let w = sub3(ro, a);
555    let a_dot = dot3(u, u);
556    let b_dot = dot3(u, v);
557    let c_dot = dot3(v, v);
558    let d_dot = dot3(u, w);
559    let e_dot = dot3(v, w);
560    let denom = a_dot * c_dot - b_dot * b_dot;
561
562    let (t, s) = if denom > 1e-6 {
563        let s_unclamped = (a_dot * e_dot - b_dot * d_dot) / denom;
564        let s_clamped = s_unclamped.clamp(0.0, 1.0);
565        let t_clamped = ((b_dot * s_clamped - d_dot) / a_dot.max(1e-6)).max(0.0);
566        (t_clamped, s_clamped)
567    } else {
568        (0.0, 0.0)
569    };
570
571    let pt_ray = add3(ro, scale3(u, t));
572    let pt_seg = add3(a, scale3(v, s));
573    let dist = length3(sub3(pt_ray, pt_seg));
574    (t, s, dist, pt_seg)
575}
576
577/// Projects a ray onto an infinite line `axis_origin + s * axis_dir`, returning signed parameter `s`.
578fn project_ray_onto_axis(
579    ro: [f32; 3],
580    rd: [f32; 3],
581    axis_origin: [f32; 3],
582    axis_dir: [f32; 3],
583) -> f32 {
584    let w = sub3(ro, axis_origin);
585    let b = dot3(rd, axis_dir);
586    let d = dot3(rd, w);
587    let e = dot3(axis_dir, w);
588    let denom = 1.0 - b * b;
589    if denom > 1e-5 {
590        (e - b * d) / denom
591    } else {
592        e
593    }
594}
595
596fn intersect_ray_plane(
597    ro: [f32; 3],
598    rd: [f32; 3],
599    plane_origin: [f32; 3],
600    plane_normal: [f32; 3],
601) -> [f32; 3] {
602    let denom = dot3(rd, plane_normal);
603    if denom.abs() > 1e-5 {
604        let t = (dot3(sub3(plane_origin, ro), plane_normal) / denom).max(0.0);
605        add3(ro, scale3(rd, t))
606    } else {
607        plane_origin
608    }
609}
610
611#[cfg(test)]
612mod tests {
613    use super::*;
614
615    #[test]
616    fn gizmo_picks_x_axis_and_computes_exact_pga_translation_motor() {
617        let gizmo = MotorGizmo3D::new(Motor::identity(), GizmoMode::Translate);
618        // Cast a ray aiming at (0.8, 0.0, 0.0) from z = 5.0
619        let hit = gizmo
620            .pick_ray([0.8, 0.02, 5.0], [0.0, 0.0, -1.0])
621            .expect("Expected ray to hit X axis arrow");
622        assert_eq!(hit.handle, GizmoHandle::AxisX);
623
624        // Drag from x = 0.8 to x = 2.3 (+1.5m along +X)
625        let delta = gizmo.compute_drag_delta(
626            GizmoHandle::AxisX,
627            [0.8, 0.0, 5.0],
628            [0.0, 0.0, -1.0],
629            [2.3, 0.0, 5.0],
630            [0.0, 0.0, -1.0],
631        );
632        let moved = delta.delta_motor.sandwich_point([0.0, 0.0, 0.0]);
633        assert!((moved[0] - 1.5).abs() < 1e-4);
634        assert!(moved[1].abs() < 1e-4);
635        assert!(moved[2].abs() < 1e-4);
636    }
637
638    #[test]
639    fn gizmo_rotate_and_motor_screw_produce_valid_pga_motors() {
640        let gizmo = MotorGizmo3D::new(Motor::identity(), GizmoMode::Rotate);
641        let hit = gizmo
642            .pick_ray([1.25, 0.0, 5.0], [0.0, 0.0, -1.0])
643            .expect("Expected ray to hit RingZ at radius 1.25");
644        assert_eq!(hit.handle, GizmoHandle::RingZ);
645
646        // Drag 90 degrees from (1, 0, 0) to (0, 1, 0) around +Z
647        let delta = gizmo.compute_drag_delta(
648            GizmoHandle::RingZ,
649            [1.0, 0.0, 5.0],
650            [0.0, 0.0, -1.0],
651            [0.0, 1.0, 5.0],
652            [0.0, 0.0, -1.0],
653        );
654        let rotated = delta.delta_motor.sandwich_point([1.0, 0.0, 0.0]);
655        assert!(rotated[0].abs() < 1e-4);
656        assert!((rotated[1] - 1.0).abs() < 1e-4);
657    }
658}