The pendulum constraint¶
A point held at a fixed length from a pivot, position-based: its projection onto the sphere of that radius about the pivot, p' = c + (p - c) L / |p - c|, the correction of a distance constraint of infinite stiffness.
|p - c| is written as the square root of the sum of the components' squares, the same law. A point at the pivot has no direction to keep; the game keeps it off the pivot. Source: Mueller et al., Position Based Dynamics (2007), 3.3. Checked against a reference in numpy, which has its own tests.
- Source
- Mueller et al., Position Based Dynamics (2007), 3.3.
- Proof
- curve, invariant, limit.
pendulum_constraint.trait
namespace lusine.integrator;
vector<float<m>, 3> pendulum_constraint(vector<float<m>, 3> position, vector<float<m>, 3> pivot, float<m> length)
{
let d = position - pivot;
let r = sqrt(d.x * d.x + d.y * d.y + d.z * d.z);
return pivot + d * (length / r);
}
A point projected back onto the sphere of its length about a pivot, the position-based correction of a distance constraint of infinite stiffness.
In [1]:
Copied!
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
from lusine.kernels._si import G0
EPS = float(np.finfo(np.float32).eps) # Trait's float is binary32
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
from lusine.kernels._si import G0
EPS = float(np.finfo(np.float32).eps) # Trait's float is binary32
The kernel¶
The law as the library states it, and as this page runs it.
In [2]:
Copied!
print(trait_run.source('integrator/pendulum_constraint.trait'))
print(trait_run.source('integrator/pendulum_constraint.trait'))
// lusine.integrator.pendulum_constraint: a point held at a fixed length from a pivot, position-based: its projection
// onto the sphere of that radius about the pivot, p' = c + (p - c) L / |p - c|, the correction of a distance
// constraint of infinite stiffness. |p - c| is written as the square root of the sum of the components' squares, the
// same law. A point at the pivot has no direction to keep; the game keeps it off the pivot.
// Source: Mueller et al., Position Based Dynamics (2007), 3.3.
// Checked against a reference in numpy, which has its own tests.
namespace lusine.integrator;
vector<float<m>, 3> pendulum_constraint(vector<float<m>, 3> position, vector<float<m>, 3> pivot, float<m> length)
{
let d = position - pivot;
let r = sqrt(d.x * d.x + d.y * d.y + d.z * d.z);
return pivot + d * (length / r);
}
The program¶
Each section runs the kernel over a lattice of items, one a case: the host sets every parameter, and main() binds the kernel over the items. The swinging pendulum's program is in its section.
In [3]:
Copied!
print(trait_run.law_program('integrator/pendulum_constraint.trait', 'pendulum_constraint'))
print(trait_run.law_program('integrator/pendulum_constraint.trait', 'pendulum_constraint'))
namespace probe;
lusine.version version = { { number = 1 } };
struct item {
host vector<float<m>, 3> position;
host vector<float<m>, 3> pivot;
host float<m> length;
vector<float<m>, 3> result_0;
};
lattice<item, 1> items(spacing = 1 m, boundary = wall);
void main() { items.result_0 = lusine.integrator.pendulum_constraint(items.position, items.pivot, items.length); }
Curve: the period at small swings¶
A bob stepped by position Verlet under gravity, the program's own kernel, and projected back to its length by the constraint each step, swings with 2 pi sqrt(L / g), over lengths from a quarter metre to two: five bobs of one run, within 0.5%.
In [4]:
Copied!
SWING = '''namespace swing;
lusine.version version = { { number = 1 } };
struct bob {
vector<float<m>, 3> position;
vector<float<m>, 3> before;
vector<float<m>, 3> free;
host vector<float<m>, 3> pivot;
host float<m> length;
};
lattice<bob, 1> bobs(spacing = 1 m, boundary = wall);
host struct { vector<float<m/s2>, 3> gravity; float<s> dt; } settings;
(vector<float<m>, 3>, vector<float<m>, 3>) verlet(vector<float<m>, 3> position, vector<float<m>, 3> before,
vector<float<m/s2>, 3> gravity, float<s> dt)
{
return (position + (position - before) + gravity * (dt * dt), position);
}
void main()
{
(bobs.free, bobs.before) = verlet(bobs.position, bobs.before, settings.gravity, settings.dt);
bobs.position = lusine.integrator.pendulum_constraint(position = bobs.free, pivot = bobs.pivot,
length = bobs.length);
}
'''
print(SWING)
lengths = np.array([0.25, 0.5, 1.0, 1.5, 2.0])
theta0, dt, steps = 0.05, 1e-3, 6000
start = np.stack([lengths * np.sin(theta0), -lengths * np.cos(theta0), np.zeros(5)], axis=1)
run = trait_run.Program(['integrator/pendulum_constraint.trait'], SWING, {'swing.bobs': [5]})
run.store('swing.settings', 'gravity', [0.0, -G0, 0.0])
run.store('swing.settings', 'dt', dt)
run.store('swing.bobs', 'pivot', np.zeros((5, 3)))
run.store('swing.bobs', 'length', lengths)
run.store('swing.bobs', 'position', start)
run.store('swing.bobs', 'before', start)
p = run.steps('swing.main', steps, [('swing.bobs', 'position')])[0] # p[k, bob, component]
x = np.concatenate([start[None, :, 0], p[:, :, 0]])
measured = []
for i in range(5):
k = np.nonzero((x[:-1, i] > 0) & (x[1:, i] <= 0))[0]
t = dt * (k + x[k, i] / (x[k, i] - x[k + 1, i])) # each downward crossing, between its steps
measured.append((t[-1] - t[0]) / (len(t) - 1))
want = 2 * np.pi * np.sqrt(lengths / G0)
print('periods', np.round(measured, 4), 'against', np.round(want, 4))
assert np.max(np.abs(np.array(measured) / want - 1)) < 0.005
r = np.linalg.norm(p, axis=2)
assert np.all(np.abs(r - lengths) <= 4 * EPS * lengths)
fig, ax = plt.subplots(figsize=(7, 3.5))
ax.plot(lengths, want, label='2 pi sqrt(L/g)')
ax.plot(lengths, measured, 'o', label='constraint')
ax.set_xlabel('L (m)')
ax.set_ylabel('period (s)')
ax.legend()
plt.show()
SWING = '''namespace swing;
lusine.version version = { { number = 1 } };
struct bob {
vector<float<m>, 3> position;
vector<float<m>, 3> before;
vector<float<m>, 3> free;
host vector<float<m>, 3> pivot;
host float<m> length;
};
lattice<bob, 1> bobs(spacing = 1 m, boundary = wall);
host struct { vector<float<m/s2>, 3> gravity; float<s> dt; } settings;
(vector<float<m>, 3>, vector<float<m>, 3>) verlet(vector<float<m>, 3> position, vector<float<m>, 3> before,
vector<float<m/s2>, 3> gravity, float<s> dt)
{
return (position + (position - before) + gravity * (dt * dt), position);
}
void main()
{
(bobs.free, bobs.before) = verlet(bobs.position, bobs.before, settings.gravity, settings.dt);
bobs.position = lusine.integrator.pendulum_constraint(position = bobs.free, pivot = bobs.pivot,
length = bobs.length);
}
'''
print(SWING)
lengths = np.array([0.25, 0.5, 1.0, 1.5, 2.0])
theta0, dt, steps = 0.05, 1e-3, 6000
start = np.stack([lengths * np.sin(theta0), -lengths * np.cos(theta0), np.zeros(5)], axis=1)
run = trait_run.Program(['integrator/pendulum_constraint.trait'], SWING, {'swing.bobs': [5]})
run.store('swing.settings', 'gravity', [0.0, -G0, 0.0])
run.store('swing.settings', 'dt', dt)
run.store('swing.bobs', 'pivot', np.zeros((5, 3)))
run.store('swing.bobs', 'length', lengths)
run.store('swing.bobs', 'position', start)
run.store('swing.bobs', 'before', start)
p = run.steps('swing.main', steps, [('swing.bobs', 'position')])[0] # p[k, bob, component]
x = np.concatenate([start[None, :, 0], p[:, :, 0]])
measured = []
for i in range(5):
k = np.nonzero((x[:-1, i] > 0) & (x[1:, i] <= 0))[0]
t = dt * (k + x[k, i] / (x[k, i] - x[k + 1, i])) # each downward crossing, between its steps
measured.append((t[-1] - t[0]) / (len(t) - 1))
want = 2 * np.pi * np.sqrt(lengths / G0)
print('periods', np.round(measured, 4), 'against', np.round(want, 4))
assert np.max(np.abs(np.array(measured) / want - 1)) < 0.005
r = np.linalg.norm(p, axis=2)
assert np.all(np.abs(r - lengths) <= 4 * EPS * lengths)
fig, ax = plt.subplots(figsize=(7, 3.5))
ax.plot(lengths, want, label='2 pi sqrt(L/g)')
ax.plot(lengths, measured, 'o', label='constraint')
ax.set_xlabel('L (m)')
ax.set_ylabel('period (s)')
ax.legend()
plt.show()
namespace swing;
lusine.version version = { { number = 1 } };
struct bob {
vector<float<m>, 3> position;
vector<float<m>, 3> before;
vector<float<m>, 3> free;
host vector<float<m>, 3> pivot;
host float<m> length;
};
lattice<bob, 1> bobs(spacing = 1 m, boundary = wall);
host struct { vector<float<m/s2>, 3> gravity; float<s> dt; } settings;
(vector<float<m>, 3>, vector<float<m>, 3>) verlet(vector<float<m>, 3> position, vector<float<m>, 3> before,
vector<float<m/s2>, 3> gravity, float<s> dt)
{
return (position + (position - before) + gravity * (dt * dt), position);
}
void main()
{
(bobs.free, bobs.before) = verlet(bobs.position, bobs.before, settings.gravity, settings.dt);
bobs.position = lusine.integrator.pendulum_constraint(position = bobs.free, pivot = bobs.pivot,
length = bobs.length);
}
periods [1.0034 1.4197 2.0048 2.4626 2.8436] against [1.0032 1.4187 2.0064 2.4573 2.8375]
Invariant: the length is held¶
Whatever the point's position and wherever the pivot, the constraint returns it at the pendulum's length from the pivot, along its own direction from it, to binary32's rounding.
In [5]:
Copied!
rng = np.random.default_rng(3)
p, c = rng.normal(size=(200, 3)) * 3, rng.normal(size=(200, 3))
L = rng.uniform(0.2, 3.0, 200)
q = trait_run.law('integrator/pendulum_constraint.trait', 'pendulum_constraint', position=p, pivot=c, length=L)
p, c, L = (np.float32(a).astype(float) for a in (p, c, L))
d, e = p - c, q - c
held = np.linalg.norm(e, axis=1)
along = np.sum(d * e, axis=1) / (np.linalg.norm(d, axis=1) * held)
assert np.all(np.abs(held - L) <= 8 * EPS * (L + np.linalg.norm(c, axis=1))) and np.all(along > 1 - 16 * EPS)
print('held at its length, along its direction, for 200 points')
rng = np.random.default_rng(3)
p, c = rng.normal(size=(200, 3)) * 3, rng.normal(size=(200, 3))
L = rng.uniform(0.2, 3.0, 200)
q = trait_run.law('integrator/pendulum_constraint.trait', 'pendulum_constraint', position=p, pivot=c, length=L)
p, c, L = (np.float32(a).astype(float) for a in (p, c, L))
d, e = p - c, q - c
held = np.linalg.norm(e, axis=1)
along = np.sum(d * e, axis=1) / (np.linalg.norm(d, axis=1) * held)
assert np.all(np.abs(held - L) <= 8 * EPS * (L + np.linalg.norm(c, axis=1))) and np.all(along > 1 - 16 * EPS)
print('held at its length, along its direction, for 200 points')
held at its length, along its direction, for 200 points
Limit: a point already at its length is left alone¶
The projection is idempotent: a point on the sphere stays where it is, and the constraint applied twice is applied once, to binary32's rounding.
In [6]:
Copied!
p = np.array([[0.6, -0.8, 0.0], [0.0, 0.0, 1.0], [2.0, 1.0, -2.0]])
c = np.zeros((3, 3))
on = trait_run.law('integrator/pendulum_constraint.trait', 'pendulum_constraint', position=p[:2], pivot=c[:2],
length=1.0)
assert np.all(np.abs(on - np.float32(p[:2])) <= 2 * EPS), on - p[:2]
q = trait_run.law('integrator/pendulum_constraint.trait', 'pendulum_constraint', position=p[2:], pivot=c[2:],
length=1.0)
qq = trait_run.law('integrator/pendulum_constraint.trait', 'pendulum_constraint', position=q, pivot=c[2:],
length=1.0)
assert np.all(np.abs(qq - q) <= 2 * EPS), qq - q
print('idempotent:', q[0], qq[0])
p = np.array([[0.6, -0.8, 0.0], [0.0, 0.0, 1.0], [2.0, 1.0, -2.0]])
c = np.zeros((3, 3))
on = trait_run.law('integrator/pendulum_constraint.trait', 'pendulum_constraint', position=p[:2], pivot=c[:2],
length=1.0)
assert np.all(np.abs(on - np.float32(p[:2])) <= 2 * EPS), on - p[:2]
q = trait_run.law('integrator/pendulum_constraint.trait', 'pendulum_constraint', position=p[2:], pivot=c[2:],
length=1.0)
qq = trait_run.law('integrator/pendulum_constraint.trait', 'pendulum_constraint', position=q, pivot=c[2:],
length=1.0)
assert np.all(np.abs(qq - q) <= 2 * EPS), qq - q
print('idempotent:', q[0], qq[0])
idempotent: [ 0.66666669 0.33333334 -0.66666669] [ 0.66666669 0.33333334 -0.66666669]