1use crate::render::telemetry::{
9 DEONTIC_LANE_BILATERAL, STANDPOINT_DID, STANDPOINT_SPECTATOR, STANDPOINT_VAULT,
10};
11
12#[derive(Clone, Copy, Debug, PartialEq)]
14pub struct Motor {
15 pub r: [f32; 4],
16 pub d: [f32; 4],
17}
18
19impl Motor {
20 #[inline]
21 pub const fn identity() -> Self {
22 Self {
23 r: [1.0, 0.0, 0.0, 0.0],
24 d: [0.0; 4],
25 }
26 }
27
28 #[inline]
29 pub fn from_rotor(r: [f32; 4]) -> Self {
30 Self { r, d: [0.0; 4] }
31 }
32}
33
34type Quat = [f32; 4];
36
37#[inline]
38fn quat_mul(a: Quat, b: Quat) -> Quat {
39 [
40 a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3],
41 a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2],
42 a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1],
43 a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0],
44 ]
45}
46
47#[inline]
48fn quat_conj(q: Quat) -> Quat {
49 [q[0], -q[1], -q[2], -q[3]]
50}
51
52#[inline]
53fn quat_add(a: Quat, b: Quat) -> Quat {
54 [a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]]
55}
56
57#[inline]
59pub fn blade4_to_quat(v: [f32; 4]) -> Quat {
60 [v[0], -v[3], -v[2], -v[1]]
61}
62
63#[inline]
65pub fn quat_to_blade4(q: Quat) -> [f32; 4] {
66 [q[0], -q[3], -q[2], -q[1]]
67}
68
69#[inline]
71pub fn motor_reverse(m: Motor) -> Motor {
72 Motor {
73 r: [m.r[0], -m.r[1], -m.r[2], -m.r[3]],
74 d: [m.d[0], -m.d[1], -m.d[2], -m.d[3]],
75 }
76}
77
78#[inline]
80pub fn motor_mul(a: Motor, b: Motor) -> Motor {
81 let qr1 = blade4_to_quat(a.r);
82 let qd1 = blade4_to_quat(a.d);
83 let qr2 = blade4_to_quat(b.r);
84 let qd2 = blade4_to_quat(b.d);
85 let qr3 = quat_mul(qr1, qr2);
86 let qd3 = quat_add(quat_mul(qr1, qd2), quat_mul(qd1, qr2));
87 Motor {
88 r: quat_to_blade4(qr3),
89 d: quat_to_blade4(qd3),
90 }
91}
92
93#[inline]
95pub fn motor_translate(v: [f32; 3]) -> Motor {
96 Motor {
97 r: [1.0, 0.0, 0.0, 0.0],
98 d: quat_to_blade4([0.0, v[0] * 0.5, v[1] * 0.5, v[2] * 0.5]),
99 }
100}
101
102#[inline]
104pub fn tensor_deontic_lane(mu: f32) -> u32 {
105 mu.round() as u32
106}
107
108#[inline]
110pub fn bilateral_pull_active(tensor_mu: f32, standpoint_class: u32) -> bool {
111 tensor_deontic_lane(tensor_mu) == DEONTIC_LANE_BILATERAL && standpoint_class >= STANDPOINT_DID
112}
113
114#[inline]
116pub fn pull_vector(node: [f32; 3], camera_eye: [f32; 3], alpha: f32, epistemic_q: f32) -> [f32; 3] {
117 let dx = camera_eye[0] - node[0];
118 let dy = camera_eye[1] - node[1];
119 let dz = camera_eye[2] - node[2];
120 let len = (dx * dx + dy * dy + dz * dz).sqrt();
121 if len < 1e-6 {
122 return [0.0; 3];
123 }
124 let gain = alpha.clamp(0.2, 1.0);
125 let delta = 0.12 * gain * epistemic_q.clamp(0.0, 1.0);
126 [dx / len * delta, dy / len * delta, dz / len * delta]
127}
128
129#[inline]
130fn approx_eq3(a: [f32; 3], b: [f32; 3], eps: f32) -> bool {
131 (a[0] - b[0]).abs() <= eps && (a[1] - b[1]).abs() <= eps && (a[2] - b[2]).abs() <= eps
132}
133
134#[inline]
138pub fn sandwich_point(m: Motor, p: [f32; 3]) -> [f32; 3] {
139 const IDENTITY_EPS: f32 = 1e-6;
140 if (m.r[0] - 1.0).abs() <= IDENTITY_EPS
141 && approx_eq3([m.r[1], m.r[2], m.r[3]], [0.0, 0.0, 0.0], IDENTITY_EPS)
142 && m.d.iter().all(|&x| x.abs() <= IDENTITY_EPS)
143 {
144 return p;
145 }
146 let qr = blade4_to_quat(m.r);
147 let qd = blade4_to_quat(m.d);
148 let qr_conj = quat_conj(qr);
149
150 let p_q: Quat = [0.0, p[0], p[1], p[2]];
151 let p_rot = quat_mul(quat_mul(qr, p_q), qr_conj);
152
153 let t_q = quat_mul(qd, qr_conj);
155 const T_SCALE: f32 = 2.0;
156 [
157 p_rot[1] + T_SCALE * t_q[1],
158 p_rot[2] + T_SCALE * t_q[2],
159 p_rot[3] + T_SCALE * t_q[3],
160 ]
161}
162
163#[inline]
166pub fn rotor_from_axis_angle(axis: [f32; 3], angle: f32) -> [f32; 4] {
167 let half = angle * 0.5;
168 let c = half.cos();
169 let s = half.sin();
170 [c, s * (-axis[2]), s * axis[1], s * (-axis[0])]
171}
172
173#[inline]
174pub fn rotor_mul(a: [f32; 4], b: [f32; 4]) -> [f32; 4] {
175 [
176 a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3],
177 a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2],
178 a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1],
179 a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0],
180 ]
181}
182
183#[inline]
185pub fn legacy_rotor_apply_vector(r: [f32; 4], v: [f32; 3]) -> [f32; 3] {
186 let q = blade4_to_quat(r);
187 let q_conj = quat_conj(q);
188 let p_q: Quat = [0.0, v[0], v[1], v[2]];
189 let out = quat_mul(quat_mul(q, p_q), q_conj);
190 [out[1], out[2], out[3]]
191}
192
193const TWO_PI: f32 = std::f32::consts::TAU;
196const MANIFOLD_COUNT: f32 = 5.0;
197const CLUSTER_COUNT: u32 = 8;
198const T_RADIAL_GAIN: f32 = 0.06;
199const ANCHOR_RING_RADIUS: f32 = 0.35;
200
201#[inline]
203pub fn cluster_id_from_sigma(sigma: f32) -> u32 {
204 let frac = sigma.fract();
205 let idx = (frac * CLUSTER_COUNT as f32).floor() as u32;
206 idx % CLUSTER_COUNT
207}
208
209#[inline]
211pub fn cluster_centroid_lattice(cluster_id: u32) -> [f32; 3] {
212 let k = cluster_id % CLUSTER_COUNT;
213 let angle = (k as f32) * TWO_PI / CLUSTER_COUNT as f32;
214 [
215 ANCHOR_RING_RADIUS * angle.cos(),
216 0.0,
217 ANCHOR_RING_RADIUS * angle.sin(),
218 ]
219}
220
221#[inline]
223pub fn motor_v_band(v: f32, node: [f32; 3], sigma: f32, time: f32, alpha: f32) -> Motor {
224 let gain = alpha.clamp(0.2, 1.0);
225 if v < 1.0 {
226 Motor::identity()
227 } else if v < 2.0 {
228 let band = v - 1.0;
229 let theta = band * TWO_PI * (time * 0.5 + sigma).sin() * gain;
230 Motor::from_rotor(rotor_from_axis_angle([0.0, 1.0, 0.0], theta))
231 } else if v < 3.0 {
232 let band = v - 2.0;
233 let len = (node[0] * node[0] + node[1] * node[1] + node[2] * node[2])
234 .sqrt()
235 .max(1e-4);
236 let dir = [node[0] / len, node[1] / len, node[2] / len];
237 let delta = T_RADIAL_GAIN * band * gain;
238 motor_translate([dir[0] * delta, dir[1] * delta, dir[2] * delta])
239 } else {
240 let centroid = cluster_centroid_lattice(cluster_id_from_sigma(sigma));
241 let blend = (v - 3.0).min(1.0) * gain;
242 motor_translate([
243 (centroid[0] - node[0]) * blend,
244 (centroid[1] - node[1]) * blend,
245 (centroid[2] - node[2]) * blend,
246 ])
247 }
248}
249
250#[inline]
251pub fn motor_rw(w: f32, alpha: f32) -> [f32; 4] {
252 let theta_w = w * (TWO_PI / MANIFOLD_COUNT);
253 let gain = alpha.clamp(0.2, 1.0);
254 rotor_from_axis_angle([0.0, 1.0, 0.0], theta_w * gain)
255}
256
257#[inline]
258pub fn motor_rq(q: f32, sigma: f32, time: f32, alpha: f32) -> [f32; 4] {
259 motor_rq_gated(q, sigma, time, alpha, STANDPOINT_SPECTATOR, 1.0)
260}
261
262#[inline]
263pub fn motor_rq_gated(
264 q: f32,
265 sigma: f32,
266 time: f32,
267 alpha: f32,
268 standpoint_class: u32,
269 epistemic_q: f32,
270) -> [f32; 4] {
271 if standpoint_class == STANDPOINT_VAULT {
272 return [1.0, 0.0, 0.0, 0.0];
273 }
274 if q <= 0.001 {
275 return [1.0, 0.0, 0.0, 0.0];
276 }
277 let gain = alpha.clamp(0.2, 1.0);
278 let mut theta_q = q * (time * 2.0 + sigma * TWO_PI).sin() * gain;
279 if standpoint_class == STANDPOINT_DID {
280 theta_q *= epistemic_q.clamp(0.0, 1.0);
281 }
282 let ax = (sigma * TWO_PI).cos();
283 let az = (sigma * TWO_PI).sin();
284 let len = (ax * ax + az * az).sqrt().max(1e-4);
285 rotor_from_axis_angle([ax / len, 0.0, az / len], theta_q)
286}
287
288#[inline]
289pub fn semantic_motor(w: f32, q: f32, sigma: f32, time: f32, alpha: f32) -> Motor {
290 semantic_motor_intrinsic(
291 0.0,
292 w,
293 q,
294 sigma,
295 time,
296 alpha,
297 [0.0; 3],
298 STANDPOINT_SPECTATOR,
299 1.0,
300 )
301}
302
303#[inline]
304pub fn semantic_motor_intrinsic(
305 tensor_v: f32,
306 w: f32,
307 q: f32,
308 sigma: f32,
309 time: f32,
310 alpha: f32,
311 node: [f32; 3],
312 standpoint_class: u32,
313 epistemic_q: f32,
314) -> Motor {
315 let r_v = motor_v_band(tensor_v, node, sigma, time, alpha);
316 let r_w = motor_rw(w, alpha);
317 let r_q = motor_rq_gated(q, sigma, time, alpha, standpoint_class, epistemic_q);
318 motor_mul(
320 Motor::from_rotor(r_w),
321 motor_mul(Motor::from_rotor(r_q), r_v),
322 )
323}
324
325#[inline]
327pub fn semantic_motor_phase2c(
328 tensor_v: f32,
329 w: f32,
330 q: f32,
331 sigma: f32,
332 time: f32,
333 alpha: f32,
334 tensor_mu: f32,
335 node: [f32; 3],
336 camera_eye: [f32; 3],
337 standpoint_class: u32,
338 epistemic_q: f32,
339) -> Motor {
340 let r_intrinsic = semantic_motor_intrinsic(
341 tensor_v,
342 w,
343 q,
344 sigma,
345 time,
346 alpha,
347 node,
348 standpoint_class,
349 epistemic_q,
350 );
351 let t_motor = if bilateral_pull_active(tensor_mu, standpoint_class) {
352 motor_translate(pull_vector(node, camera_eye, alpha, epistemic_q))
353 } else {
354 Motor::identity()
355 };
356 motor_mul(t_motor, r_intrinsic)
357}
358
359pub fn motor_to_mat4_col(m: Motor) -> [[f32; 4]; 4] {
364 let t = sandwich_point(m, [0.0, 0.0, 0.0]);
365 let cx = sandwich_point(m, [1.0, 0.0, 0.0]);
366 let cy = sandwich_point(m, [0.0, 1.0, 0.0]);
367 let cz = sandwich_point(m, [0.0, 0.0, 1.0]);
368 [
369 [cx[0] - t[0], cx[1] - t[1], cx[2] - t[2], 0.0],
370 [cy[0] - t[0], cy[1] - t[1], cy[2] - t[2], 0.0],
371 [cz[0] - t[0], cz[1] - t[1], cz[2] - t[2], 0.0],
372 [t[0], t[1], t[2], 1.0],
373 ]
374}
375
376#[cfg(test)]
377mod tests {
378 use super::*;
379
380 const EPS: f32 = 1e-5;
381
382 #[test]
383 fn identity_sandwich_is_noop() {
384 let p = [0.3, -0.7, 0.15];
385 assert!(approx_eq3(sandwich_point(Motor::identity(), p), p, EPS));
386 }
387
388 #[test]
389 fn mat4_identity_and_translation() {
390 let id = motor_to_mat4_col(Motor::identity());
391 assert_eq!(
392 id,
393 [
394 [1.0, 0.0, 0.0, 0.0],
395 [0.0, 1.0, 0.0, 0.0],
396 [0.0, 0.0, 1.0, 0.0],
397 [0.0, 0.0, 0.0, 1.0],
398 ]
399 );
400 let tm = motor_to_mat4_col(motor_translate([2.0, -3.0, 4.0]));
401 assert!((tm[3][0] - 2.0).abs() < EPS);
402 assert!((tm[3][1] + 3.0).abs() < EPS);
403 assert!((tm[3][2] - 4.0).abs() < EPS);
404 }
405
406 #[test]
407 fn d_zero_matches_legacy_quaternion_path() {
408 let fixtures: [([f32; 4], [f32; 3]); 6] = [
409 (rotor_from_axis_angle([0.0, 1.0, 0.0], 0.8), [1.0, 0.0, 0.0]),
410 (rotor_from_axis_angle([1.0, 0.0, 0.0], 1.2), [0.0, 1.0, 0.2]),
411 (
412 rotor_mul(motor_rw(2.0, 1.0), motor_rq(0.35, 0.25, 1.7, 0.9)),
413 [-0.4, 0.6, 0.1],
414 ),
415 (semantic_motor(3.0, 0.2, 0.5, 2.3, 1.0).r, [0.2, -0.3, 0.8]),
416 ([0.70710677, 0.0, 0.70710677, 0.0], [1.0, 0.0, 0.0]),
417 (
418 rotor_from_axis_angle([0.0, 0.0, 1.0], -0.5),
419 [-0.2, 0.5, 0.0],
420 ),
421 ];
422 for (r, p) in fixtures {
423 let m = Motor::from_rotor(r);
424 let legacy = legacy_rotor_apply_vector(r, p);
425 let pga = sandwich_point(m, p);
426 assert!(
427 approx_eq3(legacy, pga, EPS),
428 "legacy {:?} != pga {:?} for r {:?}",
429 legacy,
430 pga,
431 r
432 );
433 }
434 }
435
436 #[test]
437 fn motor_mul_r_matches_rotor_mul_when_d_zero() {
438 let a = motor_rw(1.0, 0.8);
439 let b = motor_rq(0.4, 0.33, 0.5, 1.0);
440 let m = motor_mul(Motor::from_rotor(a), Motor::from_rotor(b));
441 let r_legacy = rotor_mul(a, b);
442 for i in 0..4 {
443 assert!((m.r[i] - r_legacy[i]).abs() < EPS, "i={i}");
444 }
445 assert!(m.d.iter().all(|&x| x.abs() < EPS));
446 }
447
448 #[test]
449 fn semantic_fixtures_stable_against_legacy() {
450 let cases = [
451 (0.0, 0.0, 0.0, 0.0, 1.0, [0.5, 0.5, 0.0]),
452 (2.0, 0.25, 0.4, 1.0, 0.8, [0.1, -0.2, 0.3]),
453 (4.0, 0.0, 0.9, 3.14, 1.0, [-0.6, 0.0, 0.4]),
454 ];
455 for (w, q, sigma, time, alpha, p) in cases {
456 let m = semantic_motor(w, q, sigma, time, alpha);
457 let r_composed = m.r;
458 let legacy = legacy_rotor_apply_vector(r_composed, p);
459 let pga = sandwich_point(m, p);
460 assert!(
461 approx_eq3(legacy, pga, EPS),
462 "w={w} q={q} sigma={sigma}: legacy {legacy:?} pga {pga:?}"
463 );
464 }
465 }
466
467 #[test]
468 fn motor_reverse_roundtrip_rotation() {
469 let r = motor_rq(0.5, 0.2, 2.0, 1.0);
470 let m = Motor::from_rotor(r);
471 let p = [0.7, -0.1, 0.4];
472 let forward = sandwich_point(m, p);
473 let round = sandwich_point(motor_reverse(m), forward);
474 assert!(approx_eq3(round, p, EPS));
475 }
476
477 #[test]
478 fn motor_translate_shifts_point() {
479 let v = [0.08, -0.03, 0.02];
480 let p = [0.2, 0.1, -0.4];
481 let out = sandwich_point(motor_translate(v), p);
482 assert!(approx_eq3(
483 out,
484 [p[0] + v[0], p[1] + v[1], p[2] + v[2]],
485 EPS
486 ));
487 }
488
489 #[test]
490 fn bilateral_gate_off_matches_intrinsic_only() {
491 let node = [0.3, 0.1, -0.2];
492 let eye = [3.0, 1.0, 2.0];
493 let m_pull = semantic_motor_phase2c(
494 0.0,
495 2.0,
496 0.3,
497 0.5,
498 1.0,
499 0.9,
500 0.0,
501 node,
502 eye,
503 STANDPOINT_SPECTATOR,
504 1.0,
505 );
506 let m_base = semantic_motor_intrinsic(
507 0.0,
508 2.0,
509 0.3,
510 0.5,
511 1.0,
512 0.9,
513 node,
514 STANDPOINT_SPECTATOR,
515 1.0,
516 );
517 for i in 0..4 {
518 assert!((m_pull.r[i] - m_base.r[i]).abs() < EPS);
519 assert!((m_pull.d[i] - m_base.d[i]).abs() < EPS);
520 }
521 }
522
523 #[test]
524 fn bilateral_pull_shifts_toward_eye() {
525 let node = [0.0, 0.0, 0.0];
526 let eye = [3.0, 0.0, 0.0];
527 let p = [0.5, 0.0, 0.0];
528 let without = sandwich_point(
529 semantic_motor_intrinsic(0.0, 0.0, 0.0, 0.0, 0.0, 1.0, node, STANDPOINT_DID, 0.8),
530 p,
531 );
532 let with = sandwich_point(
533 semantic_motor_phase2c(
534 0.0,
535 0.0,
536 0.0,
537 0.0,
538 1.0,
539 2.0,
540 2.0,
541 node,
542 eye,
543 STANDPOINT_DID,
544 0.8,
545 ),
546 p,
547 );
548 assert!(with[0] > without[0]);
549 assert!((with[1] - without[1]).abs() < EPS);
550 assert!((with[2] - without[2]).abs() < EPS);
551 }
552
553 #[test]
554 fn v_zero_regresses_to_pre_phase3_intrinsic() {
555 let node = [0.2, -0.1, 0.3];
556 let m_v0 = semantic_motor_intrinsic(
557 0.0,
558 1.0,
559 0.2,
560 0.4,
561 1.5,
562 0.9,
563 node,
564 STANDPOINT_SPECTATOR,
565 1.0,
566 );
567 let r_w = motor_rw(1.0, 0.9);
568 let r_q = motor_rq(0.2, 0.4, 1.5, 0.9);
569 let m_legacy = motor_mul(Motor::from_rotor(r_w), Motor::from_rotor(r_q));
570 for i in 0..4 {
571 assert!((m_v0.r[i] - m_legacy.r[i]).abs() < EPS, "r[{i}]");
572 assert!((m_v0.d[i] - m_legacy.d[i]).abs() < EPS, "d[{i}]");
573 }
574 }
575
576 #[test]
577 fn cyclic_v_band_rotates_around_y() {
578 let p = [0.5, 0.0, 0.0];
579 let m = motor_v_band(1.5, p, 0.0, 0.25, 1.0);
580 let out = sandwich_point(m, p);
581 assert!(out[2].abs() > 0.01);
582 }
583
584 #[test]
585 fn hyperbolic_v_band_spreads_radially() {
586 let p = [0.4, 0.0, 0.0];
587 let m = motor_v_band(2.5, p, 0.0, 0.0, 1.0);
588 let out = sandwich_point(m, p);
589 assert!(out[0] > p[0]);
590 }
591
592 #[test]
593 fn boundary_v_band_snaps_toward_centroid() {
594 let p = [0.8, 0.1, 0.0];
595 let sigma = 0.125; let centroid = cluster_centroid_lattice(cluster_id_from_sigma(sigma));
597 let m = motor_v_band(3.2, p, sigma, 0.0, 1.0);
598 let out = sandwich_point(m, p);
599 let blend = (3.2_f32 - 3.0).min(1.0);
600 let expected = [
601 p[0] + (centroid[0] - p[0]) * blend,
602 p[1] + (centroid[1] - p[1]) * blend,
603 p[2] + (centroid[2] - p[2]) * blend,
604 ];
605 assert!(approx_eq3(out, expected, EPS));
606 }
607}