50 lines
1.3 KiB
Python
50 lines
1.3 KiB
Python
#!/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")
|