1#[repr(C)]
14#[derive(Debug, Clone, Copy, PartialEq)]
15pub struct Motor {
16 pub r: [f32; 4],
18 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 #[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 #[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 #[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 #[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 #[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 #[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 #[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#[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#[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 #[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#[derive(Debug, Clone, Copy, PartialEq)]
180pub struct GizmoPickHit {
181 pub handle: GizmoHandle,
182 pub ray_t: f32,
184 pub hit_point: [f32; 3],
186 pub handle_param: f32,
188}
189
190#[derive(Debug, Clone, Copy, PartialEq)]
192pub struct GizmoDragDelta {
193 pub delta_motor: Motor,
195 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#[derive(Debug, Clone, Copy, PartialEq)]
210pub struct MotorGizmo3D {
211 pub pivot_motor: Motor,
213 pub mode: GizmoMode,
215 pub axis_length: f32,
217 pub ring_radius: f32,
219 pub pick_tolerance: f32,
221 pub hovered_handle: Option<GizmoHandle>,
223 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 #[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 #[inline]
258 pub fn world_origin(&self) -> [f32; 3] {
259 self.pivot_motor.sandwich_point([0.0, 0.0, 0.0])
260 }
261
262 #[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 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 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 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 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 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 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 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 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
545fn 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
577fn 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 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 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 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}