test16 version as base for OOB gravity

verification/verify_frames.py

@@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+"""Deterministic basis checks for the local/world gravity convention used by test11."""
+import math
+
+
+def q_axis(axis, degrees):
+    a = math.radians(degrees) * 0.5
+    s = math.sin(a)
+    return (axis[0]*s, axis[1]*s, axis[2]*s, math.cos(a))
+
+
+def q_conj(q):
+    return (-q[0], -q[1], -q[2], q[3])
+
+
+def q_mul(a, b):
+    ax, ay, az, aw = a; bx, by, bz, bw = b
+    return (
+        aw*bx + ax*bw + ay*bz - az*by,
+        aw*by - ax*bz + ay*bw + az*bx,
+        aw*bz + ax*by - ay*bx + az*bw,
+        aw*bw - ax*bx - ay*by - az*bz,
+    )
+
+
+def rotate(q, v):
+    r = q_mul(q_mul(q, (v[0], v[1], v[2], 0.0)), q_conj(q))
+    return r[:3]
+
+
+def close(a, b, eps=1e-9):
+    return all(abs(x-y) <= eps for x, y in zip(a, b))
+
+
+UP = (0.0, 1.0, 0.0)
+DOWN = (0.0, -1.0, 0.0)
+for angle in (0, 20, 90, 180):
+    q = q_axis((1.0, 0.0, 0.0), angle)
+    world_up = rotate(q, UP)
+    world_gravity = rotate(q, DOWN)
+    assert close(rotate(q_conj(q), world_up), UP)
+    assert close(rotate(q_conj(q), world_gravity), DOWN)
+    assert close(tuple(-x for x in world_up), world_gravity)
+    print(f"X {angle:3d}: up={world_up!r}, gravity={world_gravity!r}")
+
+q180 = q_axis((1.0, 0.0, 0.0), 180)
+assert close(rotate(q180, UP), (0.0, -1.0, 0.0))
+assert close(rotate(q180, DOWN), (0.0, 1.0, 0.0))
+print("OK: at 180 degrees jump points world-down and gravity points world-up")