Stepping an orientation¶

A body's orientation stepped by its angular velocity over dt, the exact turn for a constant angular velocity: q' = normalize(dq q), dq = (sin(|w| dt / 2) w / |w|, cos(|w| dt / 2)) the turn by the angle |w| dt about w's axis, w in the world's frame.

q a unit quaternion (x, y, z, w); the product renormalised so its length's roundings never grow. Never the first-order q + (1/2) w q dt, which leaves unit length. No turn (w = 0) leaves q as it is. Source: Shoemake, Animating Rotation with Quaternion Curves (SIGGRAPH 1985); Kuipers, Quaternions and Rotation Sequences (1999), 11.1 (the exponential of a pure quaternion). Checked against a reference in numpy, which has its own tests.

Source

Shoemake (SIGGRAPH 1985); Kuipers (1999), 11.1.

Proof

reference, curve, limit.

orientation.trait

namespace lusine.integrator;

vector<float, 4> orientation(vector<float, 4> orientation, vector<float<rad/s>, 3> angular_velocity, float<s> dt)
{
  let rate = sqrt(dot(angular_velocity, angular_velocity));
  if (rate <= 0) return orientation;
  let half = 0.5 * rate * dt;
  let a = angular_velocity * (sin(half) / rate);
  let aw = cos(half);
  let b = vector<float, 3>(orientation.x, orientation.y, orientation.z);
  let bw = orientation.w;
  let v = (b * aw + a * bw) + cross(a, b);
  let w = aw * bw - dot(a, b);
  return normalize(vector<float, 4>(v.x, v.y, v.z, w));
}

A body's orientation stepped by its angular velocity, the exact turn for a constant one, normalize(dq q), never the first-order q + (1/2) w q dt that leaves unit length.

In [1]:

import os, sys
root = os.getcwd()
while not os.path.isfile(os.path.join(root, 'docsite.json')):
    root = os.path.dirname(root)
sys.path.insert(0, os.path.join(root, 'tools'))
import numpy as np
import matplotlib.pyplot as plt
from lusine import trait_run
F32 = np.float32
U = float(np.finfo(F32).eps) / 2        # binary32's unit roundoff, 2^-24
rng = np.random.default_rng(2026)

import os, sys root = os.getcwd() while not os.path.isfile(os.path.join(root, 'docsite.json')): root = os.path.dirname(root) sys.path.insert(0, os.path.join(root, 'tools')) import numpy as np import matplotlib.pyplot as plt from lusine import trait_run F32 = np.float32 U = float(np.finfo(F32).eps) / 2 # binary32's unit roundoff, 2^-24 rng = np.random.default_rng(2026)

The kernel¶

The law as the library states it.

In [2]:

print(trait_run.source('integrator/orientation.trait'))

print(trait_run.source('integrator/orientation.trait'))

// lusine.integrator.orientation: a body's orientation stepped by its angular velocity over dt, the exact turn for a
// constant angular velocity: q' = normalize(dq q), dq = (sin(|w| dt / 2) w / |w|, cos(|w| dt / 2)) the turn by the
// angle |w| dt about w's axis, w in the world's frame. q a unit quaternion (x, y, z, w); the product renormalised so
// its length's roundings never grow. Never the first-order q + (1/2) w q dt, which leaves unit length. No turn (w = 0)
// leaves q as it is.
// Source: Shoemake, Animating Rotation with Quaternion Curves (SIGGRAPH 1985); Kuipers, Quaternions and Rotation
// Sequences (1999), 11.1 (the exponential of a pure quaternion).
// Checked against a reference in numpy, which has its own tests.

namespace lusine.integrator;

vector<float, 4> orientation(vector<float, 4> orientation, vector<float<rad/s>, 3> angular_velocity, float<s> dt)
{
  let rate = sqrt(dot(angular_velocity, angular_velocity));
  if (rate <= 0) return orientation;
  let half = 0.5 * rate * dt;
  let a = angular_velocity * (sin(half) / rate);
  let aw = cos(half);
  let b = vector<float, 3>(orientation.x, orientation.y, orientation.z);
  let bw = orientation.w;
  let v = (b * aw + a * bw) + cross(a, b);
  let w = aw * bw - dot(a, b);
  return normalize(vector<float, 4>(v.x, v.y, v.z, w));
}

The program¶

Each section below runs the kernel over a lattice of items, one a case: the host sets every parameter, and main() binds the kernel over the items.

In [3]:

print(trait_run.law_program('integrator/orientation.trait', 'orientation'))

print(trait_run.law_program('integrator/orientation.trait', 'orientation'))

namespace probe;

lusine.version version = { { number = 1 } };

struct item {
  host vector<float, 4> orientation;
  host vector<float<rad/s>, 3> angular_velocity;
  host float<s> dt;
  vector<float, 4> result_0;
};
lattice<item, 1> items(spacing = 1 m, boundary = wall);

void main() { items.result_0 = lusine.integrator.orientation(items.orientation, items.angular_velocity, items.dt); }

In [4]:

from lusine.kernels.integrator.orientation import orientation as reference
from lusine.kernels.kinematics import _quaternion as Q

from lusine.kernels.integrator.orientation import orientation as reference from lusine.kernels.kinematics import _quaternion as Q

Reference: steps at random¶

Orientations and spins at random, a step of 1/60 s: the kernel's within binary32's roundings of the reference's (the sign of a quaternion aside: q and -q are one turn).

In [5]:

qs = F32(Q.random(rng, 400)).astype(float)
ws = F32(rng.normal(0.0, 20.0, (400, 3))).astype(float)
got = trait_run.law('integrator/orientation.trait', 'orientation', orientation=qs, angular_velocity=ws, dt=1 / 60)
want = reference(qs, ws, float(F32(1 / 60)))
err = np.max(np.minimum(np.linalg.norm(got - want, axis=1), np.linalg.norm(got + want, axis=1)))
assert err <= 1e-5, err
print(f'{len(qs)} steps within {err:.2g}')

qs = F32(Q.random(rng, 400)).astype(float) ws = F32(rng.normal(0.0, 20.0, (400, 3))).astype(float) got = trait_run.law('integrator/orientation.trait', 'orientation', orientation=qs, angular_velocity=ws, dt=1 / 60) want = reference(qs, ws, float(F32(1 / 60))) err = np.max(np.minimum(np.linalg.norm(got - want, axis=1), np.linalg.norm(got + want, axis=1))) assert err <= 1e-5, err print(f'{len(qs)} steps within {err:.2g}')

400 steps within 1.4e-07

Curve: a spin, round and back¶

A body spinning at 5 rad/s about (0, 3, 4) stepped 600 times through one period, 2 pi / 5 s: its +x axis traces a circle about the spin's axis and comes back where it began, the quaternion still of unit length.

In [6]:

PROGRAM = '''namespace spun;

lusine.version version = { { number = 1 } };

struct body { vector<float, 4> orientation; vector<float, 3> x_axis; };
lattice<body, 1> bodies(spacing = 1 m, boundary = wall);
host struct { vector<float<rad/s>, 3> spin; float<s> dt; } settings;

void main()
{
  bodies.orientation = lusine.integrator.orientation(orientation = bodies.orientation, angular_velocity = settings.spin,
                                                     dt = settings.dt);
  bodies.x_axis = lusine.kinematics.rotate(orientation = bodies.orientation, value = vector<float, 3>(1.0, 0.0, 0.0));
}
'''
run = trait_run.Program(['integrator/orientation.trait', 'kinematics/rotate.trait'], PROGRAM, {'spun.bodies': [1]})
run.store('spun.settings', 'spin', [0.0, 3.0, 4.0])
run.store('spun.settings', 'dt', 2 * np.pi / 5 / 600)
run.store('spun.bodies', 'orientation', [[0.0, 0.0, 0.0, 1.0]])
q, x = run.steps('spun.main', 600, [('spun.bodies', 'orientation'), ('spun.bodies', 'x_axis')])
q, x = q.reshape(600, 4), x.reshape(600, 3)
assert np.max(np.abs(np.linalg.norm(q, axis=1) - 1)) <= 8 * U
assert np.max(np.abs(x[-1] - [1, 0, 0])) <= 2e-5, x[-1]
axis = np.array([0.0, 0.6, 0.8])
assert np.max(np.abs(x @ axis - 0.0)) <= 2e-5          # +x is across the axis: it stays in the plane across it
fig = plt.figure(figsize=(4, 4)); ax = fig.add_subplot(projection='3d')
ax.plot(*x.T); ax.quiver(0, 0, 0, *axis, color='k')
plt.show()
print(f'back to {x[-1].round(6)} after one period; |q| within {np.max(np.abs(np.linalg.norm(q, axis=1) - 1)):.1g} of 1')

PROGRAM = '''namespace spun; lusine.version version = { { number = 1 } }; struct body { vector<float, 4> orientation; vector<float, 3> x_axis; }; lattice<body, 1> bodies(spacing = 1 m, boundary = wall); host struct { vector<float<rad/s>, 3> spin; float<s> dt; } settings; void main() { bodies.orientation = lusine.integrator.orientation(orientation = bodies.orientation, angular_velocity = settings.spin, dt = settings.dt); bodies.x_axis = lusine.kinematics.rotate(orientation = bodies.orientation, value = vector<float, 3>(1.0, 0.0, 0.0)); } ''' run = trait_run.Program(['integrator/orientation.trait', 'kinematics/rotate.trait'], PROGRAM, {'spun.bodies': [1]}) run.store('spun.settings', 'spin', [0.0, 3.0, 4.0]) run.store('spun.settings', 'dt', 2 * np.pi / 5 / 600) run.store('spun.bodies', 'orientation', [[0.0, 0.0, 0.0, 1.0]]) q, x = run.steps('spun.main', 600, [('spun.bodies', 'orientation'), ('spun.bodies', 'x_axis')]) q, x = q.reshape(600, 4), x.reshape(600, 3) assert np.max(np.abs(np.linalg.norm(q, axis=1) - 1)) <= 8 * U assert np.max(np.abs(x[-1] - [1, 0, 0])) <= 2e-5, x[-1] axis = np.array([0.0, 0.6, 0.8]) assert np.max(np.abs(x @ axis - 0.0)) <= 2e-5 # +x is across the axis: it stays in the plane across it fig = plt.figure(figsize=(4, 4)); ax = fig.add_subplot(projection='3d') ax.plot(*x.T); ax.quiver(0, 0, 0, *axis, color='k') plt.show() print(f'back to {x[-1].round(6)} after one period; |q| within {np.max(np.abs(np.linalg.norm(q, axis=1) - 1)):.1g} of 1')

back to [ 1.  0. -0.] after one period; |q| within 1e-07 of 1

Limit: no spin¶

No angular velocity leaves the orientation exactly as it was.

In [7]:

same = trait_run.law('integrator/orientation.trait', 'orientation', orientation=qs, angular_velocity=np.zeros((400, 3)),
                     dt=1 / 60)
assert np.array_equal(same, qs)
print('unchanged')

same = trait_run.law('integrator/orientation.trait', 'orientation', orientation=qs, angular_velocity=np.zeros((400, 3)), dt=1 / 60) assert np.array_equal(same, qs) print('unchanged')

unchanged