You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
Standalone 1-DOF angular velocity drive around an axis, with a torque-bounded impulse. Used without any other rotational constraint to spin one body relative to another (e.g. a turret, a powered wheel around a hinge-free axis).
Same row shape as the hinge motor in hinge_axis.md, but used on its own (not composed into a hinge). No alignment, no limit -- just a velocity target with a cap.
Setup
a -- world-frame axis to spin around (attached to one of the bodies, or shared after alignment)
3-DOF anchor coincidence. Anchor point on A and anchor point on B are forced to be the same world point. All three relative rotations are free (no orientation lock).
Composes three plane_setup calls -- one per world axis of the anchor separation.
Setup
Body A carries:
rA -- anchor offset from A's com (attached to A)
Body B carries:
rB -- anchor offset from B's com (attached to B)
Derivation
Position-level:
C : x̂ . p = 0
ŷ . p = 0
ẑ . p = 0
where p = (xB + rB) - (xA + rA) and x̂, ŷ, ẑ are the world axes. Each scalar constraint is a plane constraint with a fixed world axis. The derivation per row is exactly the plane derivation with t = x̂ etc.
Jacobian (per row)
With sA = p + rA = pB - xA:
J_row_i = [-e_i, e_i x sA, e_i, rB x e_i] for i = 0, 1, 2
Effective Mass
Could be computed as a 3x3 block (tighter convergence, one matrix invert per step) or as three independent scalar rows (cheap, iterates to the same answer). This file shows the scalar-row version; switching to a 3x3 block is the same math as ball-socket derivations in classic references.
Example Code
Ball-socket joint
Owns three inline rows, one per world axis of the anchor separation.
Scalar triple product (stp): a . (b x c) = b . (c x a) = c . (a x b)
A = effective mass or J * M^-1 * J^t, symmetric semi-positive-definite (SPD)
J is Jacobian
C = ... = 0 is a position level constraint
C* = ... = 0 is a velocity level constraint (time derivative of C)
Common Time Derivatives
*xA = vA
r* = wA x rA
*pA = vA + (wA x rA)
*p = (vB + (wB x rB) - [vA + (wA x rA)])
(a . b)* = a* . b + a . b* product rule
(a x b)* = a* x b + a x b* product rule
|x|* = (x . x*) / |x|
Example Code
A fully generic world loop: every active row goes through the same row_solve. No per-joint dispatch, no function pointers. Joints own their Rows inline; the world
keeps a pointer array to whichever rows are active this step. Row carries pointers to its bodies so the iteration is self-contained.
typedefstructBody
{
v3v;
v3w;
floatinv_m;
m3inv_I;
} Body;
staticinlinevoidapply_impulse(Body*A, Body*B, v3JvA, v3JwA, v3JvB, v3JwB, floatdlambda)
{
A->v=add(A->v, scale(JvA, dlambda*A->inv_m));
A->w=add(A->w, mul(A->inv_I, scale(JwA, dlambda)));
B->v=add(B->v, scale(JvB, dlambda*B->inv_m));
B->w=add(B->w, mul(B->inv_I, scale(JwB, dlambda)));
}
staticinlinefloatclamp(float*accum, floatdlambda, floatlo, floathi)
{
floatnew_accum=*accum+dlambda;
if (new_accum<lo) new_accum=lo;
if (new_accum>hi) new_accum=hi;
floatapplied=new_accum-*accum;
*accum=new_accum;
returnapplied;
}
typedefstructRow
{
Body*A, *B;
v3JvA, JwA, JvB, JwB;
floatinv_A;
floatbias;
floataccum;
floatlo, hi;
} Row;
staticvoidrow_solve(Row*row)
{
Body*A=row->A;
Body*B=row->B;
floatCdot=dot(row->JvA, A->v) +dot(row->JwA, A->w)
+dot(row->JvB, B->v) +dot(row->JwB, B->w);
floatdlambda_raw=-(Cdot+row->bias) *row->inv_A;
floatdlambda=clamp(&row->accum, dlambda_raw, row->lo, row->hi);
apply_impulse(A, B, row->JvA, row->JwA, row->JvB, row->JwB, dlambda);
}
// Shared axis-family Jacobian setup: J = [-a, a x sA, a, rB x a]// Computes effective mass from the two bodies' inv_m and inv_I.// Assumes row->A and row->B are already set.staticvoidrow_setup_axis(Row*row, v3axis, v3sA, v3rB)
{
row->JvA=neg(axis);
row->JwA=cross(axis, sA);
row->JvB=axis;
row->JwB=cross(rB, axis);
v3IA_cA=mul(row->A->inv_I, row->JwA);
v3IB_cB=mul(row->B->inv_I, row->JwB);
floatmass=row->A->inv_m+row->B->inv_m+dot(row->JwA, IA_cA) +dot(row->JwB, IB_cB);
row->inv_A=1.0f / mass;
}
typedefstructWorld
{
Body*bodies;
intn_bodies;
// Joint storage -- one typed array per joint kind.PrismaticJoint*prismatic; intn_prismatic;
// BallSocketJoint *ball_socket; int n_ball_socket;// HingeJoint *hinge; int n_hinge;// ...// Rebuilt each step: pointers to every row that should iterate.Row**active_rows;
intn_active_rows;
intcap_active_rows;
} World;
// Per-joint context passed into each constraint primitive. Bundles the// bodies, axis-family offsets, and step parameters so primitive call sites// stay short. `sA` and `rB` are ignored by non-axis primitives (e.g. orientation).typedefstructSetup
{
World*w;
Body*A, *B;
v3sA, rB;
floatbeta, dt;
} Setup;
staticinlinevoidpush_row(Setup*s, Row*row)
{
s->w->active_rows[s->w->n_active_rows++] =row;
}
staticvoidworld_step(World*w, floatdt)
{
floatbeta=0.2f;
intn_iter=8;
// 1. integrate external forces into velocitiesfor (inti=0; i<w->n_bodies; i++) integrate_forces(&w->bodies[i], dt);
// 2. setup -- one explicit loop per joint type, no dispatchw->n_active_rows=0;
for (inti=0; i<w->n_prismatic; i++) prismatic_setup(&w->prismatic[i], w, beta, dt);
// for (int i = 0; i < w->n_ball_socket; i++) ball_socket_setup(&w->ball_socket[i], w, beta, dt);// for (int i = 0; i < w->n_hinge; i++) hinge_setup(&w->hinge[i], w, beta, dt);// 3. solve -- fully generic, one call per row per iterationfor (intiter=0; iter<n_iter; iter++) {
for (inti=0; i<w->n_active_rows; i++) row_solve(w->active_rows[i]);
}
// 4. integrate velocities into positions/orientationsfor (inti=0; i<w->n_bodies; i++) integrate_positions(&w->bodies[i], dt);
}
Pluggable primitives
Each constraint .md file contributes either:
A setup primitive with signature <name>_setup(Setup *s, Row *row, <specifics>) that fills a Row and calls push_row(s, row) at the end. Used when the constraint has a new Jacobian shape. Example: plane.md contributes plane_setup, limit_setup, motor_setup.
A joint that composes primitives (its own and others'). The joint owns its Row fields inline, writes a <joint>_setup(<J> *j, World *w, float beta, float dt) function, and adds one line to world_step's setup pass.
Typical joint setup shape
staticvoid<joint>_setup(<JointStruct>*j, World*w, floatbeta, floatdt)
{
// refresh world-frame axes, offsets, anchors from A/B state (not shown)Setups= { w, j->A, j->B, j->sA, j->rB, beta, dt };
// Call primitives -- each fills the row and pushes it to active_rows.plane_setup(&s, &j->row_t0, j->t0, dot(j->t0, p));
limit_setup(&s, &j->row_limit, j->a, dot(j->a, p), j->s_min, j->s_max);
// ... etc.
}
Adding a new Joint Type
If the constraint has a new Jacobian shape, add a <name>_setup(Setup*, Row*, ...) primitive in its .md.
Define the joint struct with inline Row fields -- one Row per scalar constraint the joint produces.
Write <joint>_setup that builds a Setup and calls primitives in sequence.
Add one <joint>_setup loop in step 2 of world_step.
Step 3 of world_step (the iteration) never changes. Equality, inequality, motor, orientation, and future row kinds all flow through row_solve.
Constraint Primitives
Each file derives one scalar-constraint family (not a full joint) and contributes one or more _setup primitives that any joint can compose.
Scalar primitives
File
Primitives
Rows
Notes
plane.md
plane_setup, limit_setup, motor_setup
1
Translation along one axis (equality / limit / motor)
depth >= 0 where depth = -(pB_contact - pA_contact) . n is the penetration (positive = overlapping). One scalar row per contact, one-sided clamp λ >= 0 (contacts push apart, never pull together). Optionally adds a restitution term for bounces.
Structurally this is a plane constraint with the contact normal n as the direction, clamped unilateral, and with restitution baked into the bias.
Setup
Per contact:
n -- world-space contact normal (points from A into B, or by engine convention)
rA, rB -- offsets from each body's com to the contact point
sA = rA + p where p = pB_contact - pA_contact is the separation at the contact point (at t=0 this is usually just rA + p ≈ rA if the contact is at the world contact point on A's surface)
depth -- current penetration (positive when interpenetrating)
e -- coefficient of restitution (0 for inelastic, 1 for perfectly elastic)
Position-level Constraint
C = -n . p = depth >= 0 // p = pB_contact - pA_contact
The sign depends on convention: here n is chosen so that n . p < 0 when interpenetrating, so -n . p = depth > 0 during overlap. Adjust signs to your engine's convention.
Derivation
Identical in shape to plane with t := -n, or equivalently plane with t := n and a sign flip at the end. The velocity form uses the normal velocity at the contact point:
v_n = n . (vB + wB x rB - vA - wA x rA) // normal component of relative vel
If v_n > 0, bodies are separating (contact opening). If v_n < 0, they're approaching (worsening).
Writing with the plane-family Jacobian (using n as the axis, so that positive λ pushes B in +n relative to A -- separating):
C*_contact = -v_n // since C = -n . p, dC/dt = -n . p* = -v_n
= n . vA - wA . (n x sA) - n . vB - wB . (rB x n)
Actually the convention that's cleanest for the solver: use J = J_axis(n) (same as plane), but the active side is "inequality pushing to separate".
Jacobian
Using plane-family with axis n:
J = [-n, n x sA, n, rB x n]
This gives J . u = v_n (normal velocity, positive when separating).
Bias
Two contributions: positional correction (Baumgarte) and restitution.
Baumgarte: drive depth toward 0. Using sign rule b = (beta/dt) * C:
Note: v_n_incoming is the normal velocity at contact onset (before this step's integration), computed by the caller. depth, n, sA, rB come from the collision detector.
|p| - L = 0 keeps anchors on A and B a fixed distance L apart. One scalar row. Used for rigid links, welded segments, and stick constraints. A rope |p| <= L variant uses a signed-λ clamp.
Setup
Bodies carry their anchor offsets rA, rB. Joint stores:
L -- target distance
Derivation
Position-level constraint:
C = |p| - L = 0
= |pB - pA| - L = 0
Time-differentiate. Use recipe R7: |x|* = (x . x*) / |x|. Let n = p / |p| (unit vector from A's anchor toward B's anchor):
C* = (p . p*) / |p|
= n . p*
= n . ( (vB + wB x rB) - (vA + wA x rA) )
This is structurally identical to the plane constraint with t := n. The only distinction: n is dynamic (depends on configuration), not attached to a body. Since we freeze geometry at the start of the step, n is a constant within the step and the derivation proceeds exactly as in plane.md.
Jacobian
Apply the plane derivation's result with t replaced by n:
J = [-n, n x sA, n, rB x n]
Bias
b = (beta / dt) * (|p| - L)
Residual
residual = -n . vA + wA . (n x sA) + n . vB + wB . (rB x n) + (beta/dt)(|p| - L)
Effective mass
Same form as plane with n replacing t:
A = (1/mA + 1/mB)
+ (n x sA)^T . IA^-1 . (n x sA)
+ (rB x n)^T . IB^-1 . (rB x n)
Impulse Application
Same as plane with n replacing t:
vA <- vA - (Δλ / mA) * n
wA <- wA + Δλ * IA^-1 . (n x sA)
vB <- vB + (Δλ / mB) * n
wB <- wB + Δλ * IB^-1 . (rB x n)
Example Code
Distance setup
Compute n from current configuration, then delegate to plane_setup. The distance constraint is literally a plane constraint whose normal is the separation direction.
6-DOF lock: A and B are welded -- no relative translation at the anchors, no relative rotation. Essentially a prismatic with zero slide range, or equivalently a ball-socket plus an orientation lock.
Composes existing primitives:
plane_setup x 3 for anchor coincidence (or equivalently, ball-socket)
orientation_lock_setup for full rotational lock
Total: 6 rows, all equality.
Setup
Body A carries:
rA -- anchor offset from A's com
Body B carries:
rB -- anchor offset from B's com
Plus:
q_ref -- reference relative rotation captured at creation
Patch friction treats a whole contact manifold as a single friction interaction, not N independent Coulomb cones. Per manifold:
2 tangent rows anchored at the patch centroid (resist sliding)
1 twist row about the contact normal (resist spinning)
1 rolling row (optional; resists rolling)
Total: 3 to 4 rows per manifold, independent of contact count. Clamps couple to the total normal impulse Λ_n = Σ λ_n summed over the contact rows in the manifold, recomputed each iteration.
Why patch over per-contact Coulomb
Constant row count: 3 per manifold vs 2 * N for per-contact. A box on a plane has N=4 → 8 friction rows under Coulomb, 3 under patch.
Flat resting contacts stop "fighting": four independent per-contact cones around a box bottom can give surprising rotational drift. One patch-level pair of rows at the centroid doesn't.
Captures twist: a plate spinning on a table is a real effect that per-contact Coulomb misses -- it needs a row about the contact normal.
Matches what friction physically is: one tangential interaction at one surface patch, not N.
Setup
Per manifold:
n -- contact normal (shared by all contacts)
t1, t2 -- tangent basis perpendicular to n
centroid_rA = (1/N) Σ (contact_point_i - xA) -- patch centroid offset from A's com
centroid_rB = (1/N) Σ (contact_point_i - xB) -- same for B
patch_radius -- effective twist lever arm, typically (2/3) * sqrt(patch_area / π)
μ -- combined friction coefficient for the pair
μ_roll -- combined rolling friction coefficient (0 disables)
lambda_n_total -- scalar that holds Σ λ_n for this manifold, refreshed each iteration
The patch references the contact rows in the same manifold so it can sum their accum values each iteration.
Derivation: Tangent Rows
Velocity at the patch centroid:
v_cA = vA + wA x centroid_rA
v_cB = vB + wB x centroid_rB
Tangent velocity at the patch:
C*_k = t_k . (v_cB - v_cA) for k = 1, 2
This is structurally the plane derivation with centroid offsets in place of per-contact offsets. Same Jacobian shape:
J_tk = [-t_k, t_k x centroid_rA, t_k, centroid_rB x t_k]
Effective mass (same formula as plane with the centroid offsets):
A_tk = (1/mA + 1/mB)
+ (t_k x centroid_rA)^T . IA^-1 . (t_k x centroid_rA)
+ (centroid_rB x t_k)^T . IB^-1 . (centroid_rB x t_k)
Derivation: Twist Row
Twist is rotation about the contact normal at the patch. No linear component -- bodies rotating in place around n at the patch still have zero linear velocity at the patch centroid. The velocity-level form:
C*_twist = n . (wB - wA)
Jacobian:
J_twist = [0, -n, 0, n]
Effective mass (angular only):
A_twist = n . (IA^-1 + IB^-1) . n
Derivation: Rolling Row (optional)
Rolling opposes the tangential component of relative angular velocity. The axis is snapshotted at step start as the direction of current ω_tangent:
ω_rel = wB - wA
ω_tan = ω_rel - (ω_rel . n) * n -- component of ω_rel in the contact plane
axis = ω_tan / |ω_tan| -- skip row if |ω_tan| is tiny
Velocity form:
C*_roll = axis . (wB - wA)
Jacobian and effective mass mirror the twist row with axis in place of n:
The patch_radius factor on twist scales torque with the lever arm at the patch edge -- friction tangent forces times radius give the available twist torque.
Impulse Application
Tangent rows: same as plane with centroid offsets.
vA <- vA - (Δλ_tk / mA) * t_k
wA <- wA + Δλ_tk * IA^-1 . (t_k x centroid_rA)
vB <- vB + (Δλ_tk / mB) * t_k
wB <- wB + Δλ_tk * IB^-1 . (centroid_rB x t_k)
Twist row (angular only):
wA <- wA - Δλ_twist * IA^-1 . n
wB <- wB + Δλ_twist * IB^-1 . n
Rolling row: same as twist with axis in place of n.
Example Code
ContactPatch struct
Rows inline. Holds contact rows + friction rows together so the per-iteration refresh sees them all.
Builds all rows once per step. Bounds on friction rows are placeholders; they get rewritten each iteration from lambda_n_total.
staticvoidpatch_setup(Setup*s, ContactPatch*p)
{
p->A=s->A; p->B=s->B;
// Contact normal rows (one per contact point) -- use contact_setup from contact.md,// each with its own rA, rB, depth, etc. Not shown here.// for (int c = 0; c < p->contact_count; c++) contact_setup(s, &p->contact_rows[c], ...);// Tangent rows at the centroidSetupsc=*s;
sc.sA=p->centroid_rA;
sc.rB=p->centroid_rB;
plane_setup(&sc, &p->row_t1, p->t1, 0.0f); // bias = 0 anywayplane_setup(&sc, &p->row_t2, p->t2, 0.0f);
p->row_t1.bias=0.0f; // overwrite: friction has no position targetp->row_t2.bias=0.0f;
// Twist row -- angular only, axis = nm3I_sum=add(s->A->inv_I, s->B->inv_I);
Row*rtw=&p->row_twist;
rtw->A=s->A; rtw->B=s->B;
rtw->JvA= (v3){0,0,0};
rtw->JwA=neg(p->n);
rtw->JvB= (v3){0,0,0};
rtw->JwB=p->n;
rtw->inv_A=1.0f / dot(p->n, mul(I_sum, p->n));
rtw->bias=0.0f;
push_row(s, rtw);
// Rolling row -- axis = tangential ω_rel direction, snapshottedv3w_rel=sub(s->B->w, s->A->w);
v3w_tan=sub(w_rel, scale(p->n, dot(w_rel, p->n)));
floatw_tan_len2=dot(w_tan, w_tan);
p->has_rolling= (p->mu_roll>0.0f&&w_tan_len2>1e-8f);
if (p->has_rolling) {
v3axis=scale(w_tan, 1.0f / sqrtf(w_tan_len2));
p->rolling_axis=axis;
Row*rr=&p->row_roll;
rr->A=s->A; rr->B=s->B;
rr->JvA= (v3){0,0,0};
rr->JwA=neg(axis);
rr->JvB= (v3){0,0,0};
rr->JwB=axis;
rr->inv_A=1.0f / dot(axis, mul(I_sum, axis));
rr->bias=0.0f;
push_row(s, rr);
}
// Friction row lo/hi left at 0; refreshed each iteration by patch_refresh_caps.
}
Per-iteration cap refresh
Called once per iteration sweep, before the patch's friction rows are solved. The cheapest place is a patch_solve() helper that runs all the patch's rows in order: contacts, refresh, then frictions.
staticvoidpatch_refresh_caps(ContactPatch*p)
{
floattotal=0.0f;
for (intc=0; c<p->contact_count; c++) total+=p->contact_rows[c].accum;
p->lambda_n_total=total;
floatcap_t=p->mu*total;
floatcap_tw=p->mu*p->patch_radius*total;
p->row_t1.lo=-cap_t; p->row_t1.hi=cap_t;
p->row_t2.lo=-cap_t; p->row_t2.hi=cap_t;
p->row_twist.lo=-cap_tw; p->row_twist.hi=cap_tw;
if (p->has_rolling) {
floatcap_r=p->mu_roll*total;
p->row_roll.lo=-cap_r; p->row_roll.hi=cap_r;
}
}
staticvoidpatch_solve(ContactPatch*p)
{
for (intc=0; c<p->contact_count; c++) row_solve(&p->contact_rows[c]);
patch_refresh_caps(p);
row_solve(&p->row_t1);
row_solve(&p->row_t2);
row_solve(&p->row_twist);
if (p->has_rolling) row_solve(&p->row_roll);
}
Hooking into world_step
world_step gets a second loop alongside the joint-row loop:
for (intiter=0; iter<n_iter; iter++) {
for (inti=0; i<w->n_active_rows; i++) row_solve(w->active_rows[i]);
for (inti=0; i<w->n_patches; i++) patch_solve(&w->patches[i]);
}
The patches don't push rows into active_rows -- they run their own coupled sweep. That keeps the generic row_solve path simple (no cap_source pointer games) while the coupling lives inside patch_solve.
Two rows that keep body A's hinge axis aligned with body B's hinge axis. Leaves one rotational DOF free (rotation about the shared axis). Combine with two plane constraints on a shared anchor to make a full hinge joint (4 constraints, 1 remaining rotational DOF).
Setup
Body A carries:
aA -- unit hinge axis (attached to A)
t0, t1 -- two unit vectors perpendicular to aA and to each other (attached to A)
Body B carries:
aB -- unit hinge axis (attached to B)
When aligned: aA and aB are parallel, which is equivalent to both t0 . aB = 0 and t1 . aB = 0.
Position-level Constraint
Two scalar equations:
C : t0 . aB = 0
t1 . aB = 0
Derivation
Derive row 0 (row 1 is identical with t0 -> t1). Start with C = t0 . aB = 0, differentiate via product rule:
C* = t0* . aB + t0 . aB*
Substitute t0* = wA x t0 (t0 attached to A) and aB* = wB x aB (aB attached to B):
C* = (wA x t0) . aB + t0 . (wB x aB)
Apply stp to each cross-then-dot term:
(wA x t0) . aB = wA . (t0 x aB)
t0 . (wB x aB) = wB . (aB x t0) = -wB . (t0 x aB)
Combine:
C* = wA . (t0 x aB) - wB . (t0 x aB)
= (t0 x aB) . (wA - wB)
Rewrite as a linear function of u = [vA, wA, vB, wB]:
C* = (t0 x aB) . wA - (t0 x aB) . wB
Jacobian
Row 0:
J_row_0 = [0, (t0 x aB), 0, -(t0 x aB)]
Row 1 is the same with t0 -> t1:
J_row_1 = [0, (t1 x aB), 0, -(t1 x aB)]
No linear slots -- this is a purely rotational constraint.
Bias
Per row k:
b_k = (beta / dt) * (t_k . aB)
Residual
residual_k = (t_k x aB) . wA - (t_k x aB) . wB + (beta/dt)(t_k . aB)
Effective Mass
Per row k, with c := t_k x aB:
A_k = c^T . IA^-1 . c + c^T . IB^-1 . c
= c^T . (IA^-1 + IB^-1) . c
Impulse Application
Per-row Δλ_k applies to angular velocities only:
wA <- wA + Δλ_k * IA^-1 . (t_k x aB)
wB <- wB - Δλ_k * IB^-1 . (t_k x aB)
Example Code
Hinge axis setup
Fills 2 rows that lock the relative orientation around all directions perpendicular to the hinge axis.
Optional one-row constraint on the free axis. Extracts the signed relative angle around the hinge axis and clamps it to [theta_min, theta_max]. Unified signed-λ form, same shape as the axis-slide limit in plane.md but angular.
Derivation
The free-axis angular velocity is the scalar projection of relative w onto the hinge axis (world-frame, after alignment this is aB or aA, which coincide). Let a be the hinge axis:
omega_a = a . (wB - wA)
theta is the running relative angle, extracted from qA^-1 . qB (caller-supplied).
Jacobian
J = [0, -a, 0, a]
Bias (signed-λ unified)
if theta < theta_min: b = (beta / dt) * (theta - theta_min) // negative when under
λ in [0, +inf)
if theta > theta_max: b = (beta / dt) * (theta - theta_max) // positive when over
λ in (-inf, 0]
else: skip
Effective Mass
A = a . (IA^-1 + IB^-1) . a
Impulse Application
wA <- wA - Δλ * IA^-1 . a
wB <- wB + Δλ * IB^-1 . a
Locks all 3 rotational DOFs between A and B to a fixed reference. Used by fixed joints, prismatic joints, slider joints -- anything that forbids relative rotation.
Setup
Both bodies carry their orientations (qA, qB). Joint stores:
q_ref -- reference relative rotation captured at joint creation
Position-level Constraint
Relative rotation must match the reference. Use the imaginary part of the quaternion error:
Three scalar constraints, one per component. For small errors C ≈ axis * angle (rotation vector).
Derivation
Position-level error lives on the rotation manifold -- the quaternion machinery encodes "how much relative rotation has accumulated". At the velocity level, "relative rotation does not change" is just:
C* = wB - wA = 0
Three scalar equations, one per world axis. No quaternion math at the velocity level -- it collapses to "angular velocities match".
Scalar form per world axis e_i:
C*_i = e_i . (wB - wA) = 0
= -e_i . wA + e_i . wB
Jacobian
With u = [vA, wA, vB, wB], row i (for e_i one of x̂, ŷ, ẑ):
t . p = 0 removes translation perpendicular to a plane. Typically to be composed as two plane constraints orthogonal to an axis defined relative to body A for a
prismatic joint (allow sliding along one axis), while also doing a full-orientation lock.
Setup
Body A carries:
t is a unit vector (attached to A)
Derivation
Start with the position level constraint, written down by definition.
C = t . p = 0
= t . (pB - pA) = 0
= t . ([xB + rB] - [xA + rA]) = 0
Time-differentiate via product rule.
C* = t* . p + t . p*
Substitute t* = wA x t (t is attached to A). Since t is in world-space it is in motion, and has a time-derivative. Also substitute in expanded terms for both bodies
A and B.
C* = (wA x t) . p + t . p*
= (wA x t) . p + t . (vB + (wB x rB) - [vA + (wA x rA)])
Apply stp to pull out velocity term wA, and gather wA terms into one coefficient.
C* = -t . vA + wA . [(t x p) - (rA x t)] + t . vB + wB . (rB x t)
Simplify the wA coefficient. Use -(rA x t) = (t x rA), then factor t.
(t x p) - (rA x t) = (t x p) + (t x rA) = t x (p + rA)
Define sA = p + rA = pB - xA (vector from A's com to anchor on B).
C* = -t . vA + wA . (t x sA) + t . vB + wB . (rB x t)
Jacobian
With u = [vA, wA, vB, wB], read the coefficient of each velocity.
J = [-t, t x sA, t, rB x t]
Bias
b = (beta / dt) * (t . p)
Residual
residual = J . u + b
= -t . vA + wA . (t x sA) + t . vB + wB . (rB x t) + (beta/dt)(t . p)
Effective mass
A = (1/mA + 1/mB)
+ (t x sA)^T . IA^-1 . (t x sA)
+ (rB x t)^T . IB^-1 . (rB x t)
Impulse Application
Solve A * Δλ = -residual, then apply to the velocities.
vA <- vA - (Δλ / mA) * t
wA <- wA + Δλ * IA^-1 . (t x sA)
vB <- vB + (Δλ / mB) * t
wB <- wB + Δλ * IB^-1 . (rB x t)
Limits
Constrain the signed axis displacement a . p to lie in [s_min, s_max]. Each bound is a one-sided inequality -- active only when the joint is at that stop. Define
s_min, s_max with s_min <= a . p <= s_max.
Activation
Per step, check a . p against the bounds. Only one limit is active at a time.
Inside (s_min, s_max): skip, no constraint row.
a . p <= s_min: lower limit active.
a . p >= s_max: upper limit active.
Lower limit: (a . p) - s_min >= 0
J = J_axis
b = (beta / dt) * (a . p - s_min) // negative when violated
λ >= 0 // pushes a . p upward
Upper limit: s_max - (a . p) >= 0
J = -J_axis = [a, -(a x sA), -a, -(rB x a)]
b = (beta / dt) * (s_max - a . p) // negative when violated
λ >= 0 // pushes a . p downward
Sign rule is the same as for the plane bias: b = (beta / dt) * C where C is the position-level constraint value, so b is negative when the constraint is violated. This is what makes dlambda = -(Cdot + b) / A come out with the correct sign for the clamp to accept.
Unified single-row form
Keep one row with J = J_axis and select side via signed bias and signed clamp.
if a . p < s_min:
b = (beta / dt) * (a . p - s_min) // negative when under
λ in [0, +inf)
else if a . p > s_max:
b = (beta / dt) * (a . p - s_max) // positive when over
λ in (-inf, 0]
else:
skip
Motor (optional)
Same row J_axis, driving toward a target axis velocity v_target. Set bias so the residual equals Cdot - v_target:
b = -v_target
λ in [-F_max * dt, +F_max * dt] // bounded by motor force
The solver drives J . u toward v_target subject to the impulse cap.
Example Code
Each primitive takes (Setup *s, Row *row, <specifics>), fills the row, and calls push_row(s, row) at the end. Caller's joint_setup just sequences the calls.
Plane setup
Equality constraint: no clamp (lo = -INFINITY, hi = +INFINITY). tp = t . p precomputed from the current configuration.
5-DOF lock: translation along one axis is free, everything else (2 perpendicular translations, 3 rotations) is locked. Optional slide limit and motor on the free axis.
Composes existing primitives:
plane_setup x 2 for the two perpendicular translation locks
orientation_lock_setup for the 3 rotational locks
Optional limit_setup on the slide axis
Optional motor_setup on the slide axis
No new math -- just a joint wrapper that orchestrates primitive calls.
Setup
Body A carries:
a -- unit sliding axis (attached to A)
t0, t1 -- two unit vectors perpendicular to a (attached to A)
rA -- anchor offset from A's com (attached to A)
Body B carries:
rB -- anchor offset from B's com (attached to B)
Plus:
q_ref -- reference relative rotation (typically captured at joint creation)
for (inti=0; i<w->n_prismatic; i++) prismatic_setup(&w->prismatic[i], w, beta, dt);
Row count per prismatic
Without limit, without motor: 5 rows (2 trans + 3 rot)
With limit active: 6 rows
With motor: 6 rows (or 7 with limit also active)
All equality-style rows (the 2 planes + 3 orientation) push every step; the slide limit is only pushed when at a stop; motor is pushed whenever enabled.
A soft (spring-damper) constraint doesn't rigidly enforce C = 0 -- it behaves like a spring with stiffness k and damping c. The row still plugs into the same row_solve path; only bias, inv_A, and a new softness term change.
This lets joints model squishy attachments (cloth anchors, rope-with-sag, shock absorbers) and lets contacts model compliant materials.
Starting point: the rigid row
The rigid row solves Ċ + b = 0 where b = (beta/dt) * C. Residual is Ċ + b; dlambda = -residual/A; Ċ_after = -b. That's a stiff position correction that snaps C toward zero as aggressively as beta allows.
Replacing position correction with a spring-damper
We want the constraint to behave like:
m_eff * C̈ + c * Ċ + k * C = 0
An implicit discretization of this ODE (stable for any k, c) gives a velocity-level target:
Ċ_target = -alpha * C / dt
where alpha depends on k, c, dt, and m_eff. Plus an impulse-proportional "softness" term that compresses accumulated λ (represents the spring not fully resisting each step):
residual = Ċ + b + softness * lambda
The extra softness * lambda is the compliance: the more λ already applied, the more the row "gives" back.
Standard Bepu/box2d soft form (mass-aware)
Let:
K = J . M^-1 . J^T -- the raw effective mass (no softening)
den = 1 / (dt * (dt * k + c)) -- compliance denominator (from the implicit step; derives from k, c, dt)
Then:
softness = K * K * den / (1 + K * den) -- scaled by K so eff_mass * softness is dimensionless
eff_mass_soft = 1 / (K * (1 + K * den)) -- replaces inv_A
bias = -(K * den / (1 + K * den)) * (C / dt) -- sign & scaling fall out of the ODE integration
Equivalently (the nudge form from pre_solve_manifold.inc), using Ks = K * soft_val where soft_val = 1/den:
(Compare to rigid: dlambda = -(Ċ + bias) * inv_A, no softness term.)
Parameterization
Most engines expose (frequency, damping_ratio) instead of (k, c):
omega = 2 * pi * frequency
zeta = damping_ratio // 1 = critical
k = m_eff * omega^2
c = 2 * m_eff * zeta * omega
Then den = 1 / (dt * (dt * k + c)) as above. This gives a rig-friendly tuning where frequency is "how fast the spring bounces" and damping is "how quickly it settles".
Rigid rows set softness = 0 and keep inv_A as the raw 1/K. They behave exactly as before.
Example Code
Soft setup helper
Call after a normal rigid setup (e.g. plane_setup) to convert that row into a soft version with given frequency and damping_ratio. Computes the compliance factors from the rigid inv_A already written.
staticvoidsoft_row(Row*row, floatfrequency, floatdamping_ratio, floatdt)
{
floatK=1.0f / row->inv_A;
floatomega=2.0f*3.14159265f*frequency;
floatm_eff=1.0f / K;
floatk=m_eff*omega*omega;
floatc=2.0f*m_eff*damping_ratio*omega;
floatden_inv=dt* (dt*k+c);
if (den_inv<1e-12f) {
row->softness=0.0f; // infinitely stiff -- keep rigidreturn;
}
floatden=1.0f / den_inv;
floatKs=K*den;
floatone_plus=1.0f+Ks;
row->softness=K*Ks / one_plus;
row->inv_A=1.0f / (K*one_plus);
// Rescale bias: spring pulls C toward 0 at rate proportional to K/den, not beta.// Incoming bias was (beta/dt) * C; replace with the spring version:// row->bias was already set to (beta/dt) * C by the rigid setup -- rescale:row->bias *= Ks / one_plus;
}
Rigid default for most joints (anchor coincidence in hinges, ball-sockets). Feels locked.
Soft for vehicle suspension, cloth attachments, rope segments that should stretch, explosion-absorbing joints in ragdolls, "rubbery" joint limits.
Soft contact for compliant materials (soft tires, jelly). Modifies the normal contact row's bias and softness. Tangent friction and twist usually stay rigid.
Notes
Softness adds stability (implicit integration) even without damping, but you want some damping (zeta ≈ 0.5-1.0) to avoid spring oscillation.
Softness is a per-row property, set at setup. Nothing else in row_solve changes beyond the one-line softness * accum term.
Warm-starting interacts cleanly: the accumulated λ from last step represents past spring compression; the softness term keeps that from being over-applied this step.
Ragdoll-style angular limits on relative rotation between A and B. Replaces a 3-DOF orientation lock with two configurable constraints:
Swing -- cone limit on how far B's bone direction can rotate away from A's rest direction. One scalar row when active.
Twist -- limit on rotation about the bone axis. One scalar row when active.
Together they model a biological joint: shoulder, hip, wrist, knee. The bone can sweep through a cone and twist within bounds, but not exceed either. Both rows are inactive when inside their limits (no row pushed).
Decomposition
Given relative rotation q_rel = qA^-1 . qB, split into swing and twist about a chosen bone axis a_local in B's frame:
q_twist_raw = (q_rel.w, project(q_rel.xyz, a_local)) -- project the vector part onto a_local
q_twist = normalize(q_twist_raw) -- rotation about a_local
q_swing = q_rel . inverse(q_twist) -- the remaining rotation, perpendicular to a_local
One-DOF signed angle limit about the current bone axis a_w. Shape is identical to the hinge angle limit, with the axis being the (dynamic) world-frame bone direction rather than a fixed hinge axis.
Jacobian
J = [0, -a_w, 0, a_w]
Bias (signed-λ unified)
if twist_angle < twist_min: b = (beta / dt) * (twist_angle - twist_min) // negative when under
λ in [0, +inf)
if twist_angle > twist_max: b = (beta / dt) * (twist_angle - twist_max) // positive when over
λ in (-inf, 0]
else: skip
For a ragdoll "ball-and-socket with cone + twist limits" (shoulders, hips):
plane_setup x3 -- coincident anchor (3 rows of anchor separation)
swing_limit_setup -- cone limit (0 or 1 row, active only outside cone)
twist_limit_setup -- twist limit (0 or 1 row, active only outside twist range)
Compared to orientation_lock_setup (3 rows always): swing+twist gives you 1 DOF for elbow-like bending (twist) plus a cone for shoulder-like swing, without locking the joint rigid.
The iterative solver converges faster if it begins with a reasonable guess for λ instead of zero. For most persistent constraints (a joint that exists across steps, a contact that stays in place), last step's solved λ is an excellent guess for this step's.
Warm starting is "reuse row->accum from last step" -- but with two practical details:
Pre-apply the impulse to velocities at setup time. If we start iteration with accum non-zero but velocities haven't been updated to reflect that impulse, the first iteration's residual is wrong.
Match rows across steps. For joints with fixed row storage (Row fields in the joint struct) this is trivial. For contacts, collision points move and get destroyed/created, so you need feature-id matching to find "same contact as last step".
Per-row mechanics
Rigid / simple equality rows: accum persists in the inline row storage. At setup time, instead of wiping it, we apply the stored impulse to velocities first:
// Warm-start a single row: apply stored accum before iteration begins.staticvoidwarm_start_row(Row*row)
{
apply_impulse(row->A, row->B,
row->JvA, row->JwA, row->JvB, row->JwB,
row->accum);
}
Call this at the end of each <primitive>_setup (or at the end of <joint>_setup for all its rows). Then iterations proceed normally -- row_solve will compute deltas on top of the warm-started state.
Decay / scaling. Many engines scale the warm-started λ by a factor slightly less than 1 (e.g. 0.8) to damp out stale impulses, especially for contacts. Without this, a transient spike can echo across several frames.
Joint warm-start (easy case)
A joint's rows are fixed fields in the joint struct. Row identity is implicit -- row_t0 this step isrow_t0 last step. Just call warm_start_row on each of the joint's rows at the end of <joint>_setup:
staticvoidprismatic_setup(PrismaticJoint*j, World*w, floatbeta, floatdt)
{
// ... refresh geometry, call primitives ...plane_setup(&s, &j->row_t0, ...);
plane_setup(&s, &j->row_t1, ...);
limit_setup(&s, &j->row_limit, ...);
// etc.// Warm-start all rows that got pushed this step.// (Rows not pushed -- e.g. an inactive limit -- keep their accum but don't inject impulse.)warm_start_row(&j->row_t0);
warm_start_row(&j->row_t1);
if (j->limit_active) warm_start_row(&j->row_limit);
// ... etc.
}
For a limit row that was active last step but inactive this step, zero its accum (it's no longer meaningful):
if (!j->limit_active) j->row_limit.accum=0.0f;
Contact warm-start (harder case)
Contact points are born/destroyed each step as geometry changes. To carry λ across, we need to match this step's contacts to last step's.
Feature-id matching
Most collision detectors produce a feature_id per contact -- a tag identifying which face/edge/vertex of each shape generated this contact. If this step's contact has the same feature_id as a last-step contact, treat it as "the same contact" and copy its lambda_n.
for (eachnewcontactc)
for (eacholdcontactointhesamemanifold)
if (c.feature_id==o.feature_id&&c.feature_id!=0) {
c.lambda_n=o.lambda_n;
markbothasmatched; break;
}
Distance fallback
Some contacts have no reliable feature-id (contacts on smooth surfaces like spheres, or ambiguous face/face cases). Fall back to spatial matching: for each unmatched new contact, find the closest unmatched old contact within a tolerance, copy its λ.
for (eachunmatchednewcontactc)
findclosestunmatchedoldcontactowith |c.rA-o.rA|^2<tolerance;
if (oexists) c.lambda_n=o.lambda_n;
Unmatched-old spread
If some old contacts disappear but some new contacts are unmatched, divide the remaining old λ evenly among the unmatched new contacts. This keeps the total impulse roughly conserved across manifold changes.
A new shallow contact (freshly detected, barely penetrating) shouldn't inherit a large λ from a deep resting contact -- that would kick it hard. Clamp warm λ proportional to penetration:
Patch-level friction impulses (lambda_t1, lambda_t2, lambda_twist) are one-per-manifold, so they match directly (no feature-id needed). Carry them across unconditionally.
Rolling friction has a snapshotted axis; carry the λ only if the axis is still approximately aligned with the new snapshot (dot product > ~0.5), otherwise zero it. Otherwise you inject impulse in the wrong direction.
Inline in joint struct (easy): rows live in the joint, accum persists naturally between world_step calls. Nothing else needed.
Warm cache by key (for contacts): manifolds keyed by (body_a, body_b, sub_id) hashed into a cache; each step's manifold looks up its prior state and copies λ + feature_ids forward. The WarmManifold struct in nudge is exactly this.
When NOT to warm-start
Row inactive last step but active this step (e.g. a joint limit just became engaged): accum = 0. Nothing to warm-start from.
Contact with flipped normal (rare; happens in deep penetration edge cases): wipe λ.
New manifold (bodies just started touching): all λ start at 0.
Bodies at very different configurations than last step (teleportation, large single-step motion): warm-start invalid.
What you buy
For persistent contacts and joints, warm-starting typically cuts required iterations in half or more. Nudge uses 8-16 velocity iterations with warm-start; without warm-start it would need 30-60 for similar quality in a resting stack.
Example Code
Full warm-starting integration is covered in the contact/friction and joint files; each calls warm_start_row (or an equivalent) on its rows after setup. Here's the standalone helper:
Call it in <joint>_setup after the primitive calls finish, on each row that was pushed this step. For inactive rows (limits that didn't engage), either leave accum alone (it'll pick up where it left off next time the limit engages) or zero it (safer -- avoids stale impulses).