Skip to content

Instantly share code, notes, and snippets.

@RandyGaul
Last active April 20, 2026 03:19
Show Gist options
  • Select an option

  • Save RandyGaul/35478e25114d3f35585e1c97ba156fef to your computer and use it in GitHub Desktop.

Select an option

Save RandyGaul/35478e25114d3f35585e1c97ba156fef to your computer and use it in GitHub Desktop.
Constraint derivations for iterative PGS/SI physics solver

Angular Motor

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)
  • v_target -- desired scalar angular velocity (wB - wA) . a
  • T_max -- maximum torque the motor can apply; impulse cap per step is T_max * dt

Derivation

Relative angular velocity projected onto a:

C* = a . (wB - wA) = -a . wA + a . wB

Jacobian:

J = [0, -a, 0, a]

Bias and bounds are the only thing that differ from a plain 1-DOF equality row:

b = -v_target
lo = -T_max * dt
hi = +T_max * dt

Effective Mass

Scalar:

A = a . (IA^-1 + IB^-1) . a

Impulse Application

Angular only:

wA <- wA - Δλ * IA^-1 . a
wB <- wB + Δλ * IB^-1 . a

Example Code

Angular motor joint

typedef struct AngularMotorJoint
{
      Body *A, *B;
      v3 a;
      float v_target;
      float T_max;
      Row row;
} AngularMotorJoint;

static void angular_motor_setup(AngularMotorJoint *j, World *w, float beta, float dt)
{
      // Refresh j->a (if attached to a body) from orientation (not shown).
      Setup s = { w, j->A, j->B, (v3){0,0,0}, (v3){0,0,0}, beta, dt };

      Row *r = &j->row;
      r->A = s.A;  r->B = s.B;
      r->JvA = (v3){0,0,0};
      r->JwA = neg(j->a);
      r->JvB = (v3){0,0,0};
      r->JwB = j->a;
      m3 I_sum = add(s.A->inv_I, s.B->inv_I);
      r->inv_A = 1.0f / dot(j->a, mul(I_sum, j->a));
      r->bias  = -j->v_target;
      float cap = j->T_max * dt;
      r->lo = -cap;
      r->hi = cap;
      push_row(&s, r);
}

Hook into world_step

for (int i = 0; i < w->n_angular_motor; i++) angular_motor_setup(&w->angular_motor[i], w, beta, dt);

Use cases

  • Turret spinning around a vertical axis.
  • Wheel driven around its own rotation axis with a coexisting hinge (hinge handles alignment + limits; angular motor handles torque drive).
  • Ragdoll / character rigging: proportional damping of relative angular velocity between bones, with v_target = 0 and a small T_max.

Ball-Socket

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.

typedef struct BallSocketJoint
{
      Body *A, *B;
      v3 rA, rB;           // anchor offsets (world frame, refreshed per step)
      v3 sA;               // = pB - xA (derived per step)
      Row row_x, row_y, row_z;
} BallSocketJoint;

static void ball_socket_setup(BallSocketJoint *j, World *w, float beta, float dt)
{
      // refresh j->rA, j->rB from body orientations (not shown)
      v3 pA = add(j->A->x, j->rA);
      v3 pB = add(j->B->x, j->rB);
      v3 p  = sub(pB, pA);
      j->sA = sub(pB, j->A->x);

      Setup s = { w, j->A, j->B, j->sA, j->rB, beta, dt };

      plane_setup(&s, &j->row_x, (v3){1,0,0}, p.x);
      plane_setup(&s, &j->row_y, (v3){0,1,0}, p.y);
      plane_setup(&s, &j->row_z, (v3){0,0,1}, p.z);
}

Hook into world_step

for (int i = 0; i < w->n_ball_socket; i++) ball_socket_setup(&w->ball_socket[i], w, beta, dt);

Common Notation and Patterns

  • xA is com at A
  • rA is vector from com of A
  • wA angular vel of A
  • pA = xA + rA
  • pB = xB + rB
  • p = pB - pA = ([xB + rB] - [xA + rA])
  • 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.

typedef struct Body
{
      v3 v;
      v3 w;
      float inv_m;
      m3 inv_I;
} Body;

static inline void apply_impulse(Body *A, Body *B, v3 JvA, v3 JwA, v3 JvB, v3 JwB, float dlambda)
{
      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)));
}

static inline float clamp(float *accum, float dlambda, float lo, float hi)
{
      float new_accum = *accum + dlambda;
      if (new_accum < lo) new_accum = lo;
      if (new_accum > hi) new_accum = hi;
      float applied = new_accum - *accum;
      *accum = new_accum;
      return applied;
}

typedef struct Row
{
      Body *A, *B;
      v3 JvA, JwA, JvB, JwB;
      float inv_A;
      float bias;
      float accum;
      float lo, hi;
} Row;

static void row_solve(Row *row)
{
      Body *A = row->A;
      Body *B = row->B;
      float Cdot = dot(row->JvA, A->v) + dot(row->JwA, A->w)
                 + dot(row->JvB, B->v) + dot(row->JwB, B->w);
      float dlambda_raw = -(Cdot + row->bias) * row->inv_A;
      float dlambda = 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.
static void row_setup_axis(Row *row, v3 axis, v3 sA, v3 rB)
{
      row->JvA = neg(axis);
      row->JwA = cross(axis, sA);
      row->JvB = axis;
      row->JwB = cross(rB, axis);

      v3 IA_cA = mul(row->A->inv_I, row->JwA);
      v3 IB_cB = mul(row->B->inv_I, row->JwB);
      float mass = row->A->inv_m + row->B->inv_m
                 + dot(row->JwA, IA_cA) + dot(row->JwB, IB_cB);
      row->inv_A = 1.0f / mass;
}

typedef struct World
{
      Body *bodies;
      int n_bodies;

      // Joint storage -- one typed array per joint kind.
      PrismaticJoint *prismatic;   int n_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;
      int n_active_rows;
      int cap_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).
typedef struct Setup
{
      World *w;
      Body *A, *B;
      v3 sA, rB;
      float beta, dt;
} Setup;

static inline void push_row(Setup *s, Row *row)
{
      s->w->active_rows[s->w->n_active_rows++] = row;
}

static void world_step(World *w, float dt)
{
      float beta = 0.2f;
      int n_iter = 8;

      // 1. integrate external forces into velocities
      for (int i = 0; i < w->n_bodies; i++) integrate_forces(&w->bodies[i], dt);

      // 2. setup -- one explicit loop per joint type, no dispatch
      w->n_active_rows = 0;
      for (int i = 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 iteration
      for (int iter = 0; iter < n_iter; iter++) {
              for (int i = 0; i < w->n_active_rows; i++) row_solve(w->active_rows[i]);
      }

      // 4. integrate velocities into positions/orientations
      for (int i = 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

static void <joint>_setup(<JointStruct> *j, World *w, float beta, float dt)
{
      // refresh world-frame axes, offsets, anchors from A/B state (not shown)
      Setup s = { 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

  1. If the constraint has a new Jacobian shape, add a <name>_setup(Setup*, Row*, ...) primitive in its .md.
  2. Define the joint struct with inline Row fields -- one Row per scalar constraint the joint produces.
  3. Write <joint>_setup that builds a Setup and calls primitives in sequence.
  4. 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)
distance.md distance_setup, rope_setup 1 `
orientation_lock.md orientation_lock_setup 3 Full 3-DOF rotational lock between A and B
hinge_axis.md hinge_axis_setup, hinge_limit_setup, hinge_motor_setup 2+1+1 Hinge alignment; optional angle limit and motor on the free axis
swing_twist.md swing_limit_setup, twist_limit_setup 1+1 Ragdoll cone swing + twist limit
contact.md contact_setup 1 Penetration row with restitution
friction.md patch_setup, patch_refresh_caps, patch_solve 3-4 Patch friction: 2 tangent + 1 twist (+ rolling), coupled to Σλ_n

Joint wrappers

File Primitives used Rows
ball_socket.md plane_setup x3 (one per world axis of anchor separation) 3
prismatic.md plane_setup x2 + orientation_lock_setup + optional limit_setup + optional motor_setup 5-7
hinge.md (TBD) plane_setup x3 + hinge_axis_setup + optional hinge_limit_setup / hinge_motor_setup 5-7
fixed.md plane_setup x3 + orientation_lock_setup 6
angular_motor.md single-row standalone (no composition) 1

Cross-cutting topics

File Topic
soft_constraints.md Spring-damper rows: modify inv_A, bias, add softness * accum term to residual
warm_starting.md Persist accum across steps; pre-apply impulse at setup; feature-id matching for contacts

Composing Joints

Typical joint compositions (each joint lives in its own .md alongside its setup function -- just the <joint>_setup wrapper; no new math):

Joint Primitives used
Ball-socket plane_setup x3 (one per world axis of the anchor separation)
Prismatic plane_setup x2 + limit_setup + optional motor_setup + orientation_lock_setup
Hinge plane_setup x3 (anchor coincidence) + hinge_axis_setup + optional hinge_limit_setup / hinge_motor_setup
Fixed plane_setup x3 + orientation_lock_setup
Distance distance_setup
Rope rope_setup
Ragdoll (cone) plane_setup x3 + swing_limit_setup + twist_limit_setup
Contact patch contact_setup x N (one per contact point) wrapped in a ContactPatch + patch friction rows

Contact

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:

b_pos = (beta / dt) * (-depth)        // negative when penetrating

Many engines use a slop: b_pos = (beta/dt) * max(0, depth - slop) * -1, so tiny overlaps don't kick bodies apart.

Restitution: if approaching fast enough, set the target separating velocity to -e * v_n_incoming:

b_rest = e * min(0, v_n_incoming)     // only applied when v_n_incoming < -threshold

Combine (engine-specific, but typically):

b = b_pos + b_rest

Clamp

λ in [0, +inf)        // contact can only push, never pull

Effective Mass

Identical to plane with n:

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:

vA <- vA - (Δλ / mA) * n
wA <- wA + Δλ * IA^-1 . (n x sA)
vB <- vB + (Δλ / mB) * n
wB <- wB + Δλ * IB^-1 . (rB x n)

Example Code

Contact setup

static void contact_setup(Setup *s, Row *row, v3 n, float depth, float v_n_incoming, float e, float slop)
{
      row->A = s->A;
      row->B = s->B;
      row_setup_axis(row, n, s->sA, s->rB);

      float pen = depth - slop;
      float b_pos  = (pen > 0.0f) ? -(s->beta / s->dt) * pen : 0.0f;
      float b_rest = (v_n_incoming < 0.0f) ? e * v_n_incoming : 0.0f;
      row->bias = b_pos + b_rest;

      row->lo = 0.0f;
      row->hi = INFINITY;
      push_row(s, row);
}

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.

Distance

|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.

static void distance_setup(Setup *s, Row *row, v3 p, float L)
{
      float len = length(p);
      v3 n = scale(p, 1.0f / len);
      plane_setup(s, row, n, len - L);
}

Rope setup (inequality form, |p| <= L)

Use a one-sided clamp so the constraint is active only when the rope is taut (overstretched). Sign rule matches b = (beta/dt) * C with C = L - |p|.

static void rope_setup(Setup *s, Row *row, v3 p, float L)
{
      float len = length(p);
      if (len <= L) return;        // slack -- skip the row

      v3 n = scale(p, 1.0f / len);
      row->A    = s->A;
      row->B    = s->B;
      row_setup_axis(row, n, s->sA, s->rB);
      row->bias = (s->beta / s->dt) * (len - L);   // positive when taut
      row->lo   = -INFINITY;
      row->hi   = 0.0f;                            // pulls anchors together only
      push_row(s, row);
}

Fixed (Weld)

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

Example Code

Fixed joint

typedef struct FixedJoint
{
      Body *A, *B;
      v3 rA, rB, sA;
      quat q_ref;

      Row row_x, row_y, row_z;                     // anchor coincidence
      Row row_rot_x, row_rot_y, row_rot_z;         // orientation lock
} FixedJoint;

static void fixed_setup(FixedJoint *j, World *w, float beta, float dt)
{
      // Refresh world-frame rA, rB from body orientations (not shown).
      v3 pA = add(j->A->x, j->rA);
      v3 pB = add(j->B->x, j->rB);
      v3 p  = sub(pB, pA);
      j->sA = sub(pB, j->A->x);

      v3 err = quat_error(j->A->q, j->B->q, j->q_ref);

      Setup s = { w, j->A, j->B, j->sA, j->rB, beta, dt };

      plane_setup(&s, &j->row_x, (v3){1,0,0}, p.x);
      plane_setup(&s, &j->row_y, (v3){0,1,0}, p.y);
      plane_setup(&s, &j->row_z, (v3){0,0,1}, p.z);
      orientation_lock_setup(&s, &j->row_rot_x, &j->row_rot_y, &j->row_rot_z, err);
}

Hook into world_step

for (int i = 0; i < w->n_fixed; i++) fixed_setup(&w->fixed[i], w, beta, dt);

Six rows unconditionally every step. A fixed joint is the highest-row-count basic joint; use only when you actually need a full weld.

Friction (Patch)

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:

J_roll = [0, -axis, 0, axis]
A_roll = axis . (IA^-1 + IB^-1) . axis

Bias

Friction has no position-level target, so b = 0 for all four rows.

Clamps (coupled to total normal impulse)

Let Λ_n = Σ λ_n over the manifold's contact rows. Refreshed each iteration by summing contact-row accum values.

tangent 1: λ_t1     in [-μ * Λ_n,                   +μ * Λ_n]
tangent 2: λ_t2     in [-μ * Λ_n,                   +μ * Λ_n]
twist:     λ_twist  in [-μ * patch_radius * Λ_n,    +μ * patch_radius * Λ_n]
rolling:   λ_roll   in [-μ_roll * Λ_n,              +μ_roll * Λ_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.

typedef struct ContactPatch
{
      Body *A, *B;
      v3 n, t1, t2;
      v3 centroid_rA, centroid_rB;
      float patch_radius;
      float mu, mu_roll;

      int   contact_count;
      Row   contact_rows[MAX_CONTACTS];    // normal rows (one per contact point)

      Row   row_t1, row_t2;                 // patch tangent rows
      Row   row_twist;                      // patch twist row

      int   has_rolling;
      v3    rolling_axis;                   // snapshotted at step start
      Row   row_roll;

      float lambda_n_total;                 // refreshed each iteration
} ContactPatch;

Patch setup

Builds all rows once per step. Bounds on friction rows are placeholders; they get rewritten each iteration from lambda_n_total.

static void patch_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 centroid
      Setup sc = *s;
      sc.sA = p->centroid_rA;
      sc.rB = p->centroid_rB;

      plane_setup(&sc, &p->row_t1, p->t1, 0.0f);   // bias = 0 anyway
      plane_setup(&sc, &p->row_t2, p->t2, 0.0f);
      p->row_t1.bias = 0.0f;                        // overwrite: friction has no position target
      p->row_t2.bias = 0.0f;

      // Twist row -- angular only, axis = n
      m3 I_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, snapshotted
      v3 w_rel = sub(s->B->w, s->A->w);
      v3 w_tan = sub(w_rel, scale(p->n, dot(w_rel, p->n)));
      float w_tan_len2 = dot(w_tan, w_tan);
      p->has_rolling = (p->mu_roll > 0.0f && w_tan_len2 > 1e-8f);
      if (p->has_rolling) {
              v3 axis = 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.

static void patch_refresh_caps(ContactPatch *p)
{
      float total = 0.0f;
      for (int c = 0; c < p->contact_count; c++) total += p->contact_rows[c].accum;
      p->lambda_n_total = total;

      float cap_t  = p->mu * total;
      float cap_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) {
              float cap_r = p->mu_roll * total;
              p->row_roll.lo = -cap_r;  p->row_roll.hi = cap_r;
      }
}

static void patch_solve(ContactPatch *p)
{
      for (int c = 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 (int iter = 0; iter < n_iter; iter++) {
      for (int i = 0; i < w->n_active_rows; i++) row_solve(w->active_rows[i]);
      for (int i = 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.

Hinge Axis

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.

static void hinge_axis_setup(Setup *s, Row *r0, Row *r1, v3 t0, v3 t1, v3 aB)
{
      Row *rows[2] = { r0, r1 };
      v3   ts[2]   = { t0, t1 };
      m3 I_sum = add(s->A->inv_I, s->B->inv_I);

      for (int i = 0; i < 2; i++) {
              Row *r = rows[i];
              v3 c   = cross(ts[i], aB);   // t_i x aB
              r->A   = s->A;
              r->B   = s->B;
              r->JvA = (v3){0,0,0};
              r->JwA = c;
              r->JvB = (v3){0,0,0};
              r->JwB = neg(c);
              r->inv_A = 1.0f / dot(c, mul(I_sum, c));
              r->bias  = (s->beta / s->dt) * dot(ts[i], aB);
              r->lo    = -INFINITY;
              r->hi    = INFINITY;
              push_row(s, r);
      }
}

Hinge Angle Limit

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

Setup

static void hinge_limit_setup(Setup *s, Row *row, v3 a, float theta, float theta_min, float theta_max)
{
      if (theta < theta_min) {
              row->bias = (s->beta / s->dt) * (theta - theta_min);
              row->lo = 0.0f;  row->hi = INFINITY;
      } else if (theta > theta_max) {
              row->bias = (s->beta / s->dt) * (theta - theta_max);
              row->lo = -INFINITY;  row->hi = 0.0f;
      } else {
              return;
      }
      row->A = s->A;  row->B = s->B;
      row->JvA = (v3){0,0,0};
      row->JwA = neg(a);
      row->JvB = (v3){0,0,0};
      row->JwB = a;
      m3 I_sum = add(s->A->inv_I, s->B->inv_I);
      row->inv_A = 1.0f / dot(a, mul(I_sum, a));
      push_row(s, row);
}

Hinge Motor

Optional one-row constraint that drives relative angular velocity around the hinge axis toward omega_target. Bounded by torque cap T_max.

Same J and effective mass as the limit row. Differs only in bias and clamp: bias = -omega_target, clamp [-T_max * dt, +T_max * dt].

Setup

static void hinge_motor_setup(Setup *s, Row *row, v3 a, float omega_target, float T_max)
{
      row->A = s->A;  row->B = s->B;
      row->JvA = (v3){0,0,0};
      row->JwA = neg(a);
      row->JvB = (v3){0,0,0};
      row->JwB = a;
      m3 I_sum = add(s->A->inv_I, s->B->inv_I);
      row->inv_A = 1.0f / dot(a, mul(I_sum, a));
      row->bias  = -omega_target;
      float cap = T_max * s->dt;
      row->lo = -cap;
      row->hi = cap;
      push_row(s, row);
}

Orientation Lock

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:

q_err = qA^-1 . qB . q_ref^-1         // quaternion; identity when locked
C     = 2 . Im(q_err)                 // 3-vector axis-angle error (R^3)

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̂, ŷ, ẑ):

J_i = [0, -e_i, 0, e_i]

No linear slots -- pure rotational lock.

Bias

Per row:

b_i = (beta / dt) * C_i

Residual

residual_i = -e_i . wA + e_i . wB + b_i

Effective mass

Per row, a 1x1 scalar:

A_i = J_i . M^-1 . J_i^T
    = e_i . IA^-1 . e_i + e_i . IB^-1 . e_i
    = e_i . (IA^-1 + IB^-1) . e_i

Impulse Application

Per-row Δλ_i applies to angular velocities only:

wA <- wA - Δλ_i * IA^-1 . e_i
wB <- wB + Δλ_i * IB^-1 . e_i

Linear velocities untouched.

Example Code

Orientation lock setup

Fills 3 rows, one per world axis. err = 2 . Im(qA^-1 . qB . q_ref^-1) is the axis-angle error vector, computed once by the caller.

static void orientation_lock_setup(Setup *s, Row *rx, Row *ry, Row *rz, v3 err)
{
      Row *rows[3]  = { rx, ry, rz };
      v3   axes[3]  = { {1,0,0}, {0,1,0}, {0,0,1} };
      float err_c[3] = { err.x, err.y, err.z };
      m3 I_sum = add(s->A->inv_I, s->B->inv_I);

      for (int i = 0; i < 3; i++) {
              Row *r  = rows[i];
              v3   ax = axes[i];
              r->A    = s->A;
              r->B    = s->B;
              r->JvA  = (v3){0,0,0};
              r->JwA  = neg(ax);
              r->JvB  = (v3){0,0,0};
              r->JwB  = ax;
              r->inv_A = 1.0f / dot(ax, mul(I_sum, ax));
              r->bias  = (s->beta / s->dt) * err_c[i];
              r->lo    = -INFINITY;
              r->hi    = INFINITY;
              push_row(s, r);
      }
}

Plane

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.

static void plane_setup(Setup *s, Row *row, v3 t, float tp)
{
      row->A = s->A;
      row->B = s->B;
      row_setup_axis(row, t, s->sA, s->rB);
      row->bias = (s->beta / s->dt) * tp;
      row->lo = -INFINITY;
      row->hi = INFINITY;
      push_row(s, row);
}

Limit setup

Unified signed-λ form. Skips pushing the row entirely when inside the bounds.

static void limit_setup(Setup *s, Row *row, v3 a, float ap, float s_min, float s_max)
{
      if (ap < s_min) {
              row->bias = (s->beta / s->dt) * (ap - s_min);
              row->lo = 0.0f;
              row->hi = INFINITY;
      } else if (ap > s_max) {
              row->bias = (s->beta / s->dt) * (ap - s_max);
              row->lo = -INFINITY;
              row->hi = 0.0f;
      } else {
              return;
      }
      row->A = s->A;
      row->B = s->B;
      row_setup_axis(row, a, s->sA, s->rB);
      push_row(s, row);
}

Motor setup

Symmetric impulse cap (F_max * dt), bias = -v_target.

static void motor_setup(Setup *s, Row *row, v3 a, float v_target, float F_max)
{
      row->A = s->A;
      row->B = s->B;
      row_setup_axis(row, a, s->sA, s->rB);
      row->bias = -v_target;
      float cap = F_max * s->dt;
      row->lo = -cap;
      row->hi = cap;
      push_row(s, row);
}

Prismatic

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)
  • s_min, s_max -- slide limits (optional)
  • v_target, F_max -- motor parameters (optional)

Example Code

Prismatic joint

typedef struct PrismaticJoint
{
      Body *A, *B;
      v3 a, t0, t1;
      v3 rA, rB, sA;
      quat q_ref;
      float s_min, s_max;
      int has_motor;
      float v_target, F_max;

      Row row_t0, row_t1;
      Row row_limit;
      Row row_motor;
      Row row_rot_x, row_rot_y, row_rot_z;
} PrismaticJoint;

static void prismatic_setup(PrismaticJoint *j, World *w, float beta, float dt)
{
      // Refresh world-frame a, t0, t1, rA, rB from body orientations (not shown).
      v3 pA = add(j->A->x, j->rA);
      v3 pB = add(j->B->x, j->rB);
      v3 p  = sub(pB, pA);
      j->sA = sub(pB, j->A->x);

      // Orientation error: 2 * Im(qA^-1 . qB . q_ref^-1) -- caller-computed helper.
      v3 err = quat_error(j->A->q, j->B->q, j->q_ref);

      Setup s = { w, j->A, j->B, j->sA, j->rB, beta, dt };

      // Two translation locks perpendicular to the slide axis.
      plane_setup(&s, &j->row_t0, j->t0, dot(j->t0, p));
      plane_setup(&s, &j->row_t1, j->t1, dot(j->t1, p));

      // Optional slide limit.
      limit_setup(&s, &j->row_limit, j->a, dot(j->a, p), j->s_min, j->s_max);

      // Optional motor.
      if (j->has_motor) {
              motor_setup(&s, &j->row_motor, j->a, j->v_target, j->F_max);
      }

      // Full orientation lock (3 rows).
      orientation_lock_setup(&s, &j->row_rot_x, &j->row_rot_y, &j->row_rot_z, err);
}

Hook into world_step

for (int i = 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.

Soft Constraints

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:

softness = K * Ks / (1 + Ks)
eff_mass_soft = 1 / (K * (1 + Ks))

The update becomes:

residual = Ċ + bias + softness * lambda
dlambda  = eff_mass_soft * (-residual)

(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".

What changes in the Row / solver

Add one field and one line:

typedef struct Row
{
      Body *A, *B;
      v3 JvA, JwA, JvB, JwB;
      float inv_A;       // already softened if this is a soft row
      float bias;
      float softness;    // NEW: 0 for rigid rows
      float accum;
      float lo, hi;
} Row;

static void row_solve(Row *row)
{
      Body *A = row->A, *B = row->B;
      float Cdot = dot(row->JvA, A->v) + dot(row->JwA, A->w)
                 + dot(row->JvB, B->v) + dot(row->JwB, B->w);
      float dlambda_raw = -(Cdot + row->bias + row->softness * row->accum) * row->inv_A;
      float dlambda = clamp(&row->accum, dlambda_raw, row->lo, row->hi);
      apply_impulse(A, B, row->JvA, row->JwA, row->JvB, row->JwB, dlambda);
}

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.

static void soft_row(Row *row, float frequency, float damping_ratio, float dt)
{
      float K = 1.0f / row->inv_A;
      float omega = 2.0f * 3.14159265f * frequency;
      float m_eff = 1.0f / K;
      float k = m_eff * omega * omega;
      float c = 2.0f * m_eff * damping_ratio * omega;
      float den_inv = dt * (dt * k + c);
      if (den_inv < 1e-12f) {
              row->softness = 0.0f;       // infinitely stiff -- keep rigid
              return;
      }
      float den = 1.0f / den_inv;
      float Ks = K * den;
      float one_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;
}

Using it

// Soft ball-socket anchor: rigid setup, then soften.
plane_setup(&s, &j->row_x, (v3){1,0,0}, p.x);
soft_row(&j->row_x, 5.0f, 0.7f, dt);       // 5 Hz, damping ratio 0.7

plane_setup(&s, &j->row_y, (v3){0,1,0}, p.y);
soft_row(&j->row_y, 5.0f, 0.7f, dt);

plane_setup(&s, &j->row_z, (v3){0,0,1}, p.z);
soft_row(&j->row_z, 5.0f, 0.7f, dt);

When to use

  • 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.

Swing + Twist

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

Extract angles:

swing_angle = 2 . atan2( |q_swing.xyz|, q_swing.w )                    -- always >= 0
twist_angle = 2 . atan2( q_twist.xyz . a_local, q_twist.w )             -- signed

Swing (cone limit)

World-frame directions

a_w     = qB . a_local            -- bone direction (attached to B)
rest_w  = qA . rest_local         -- rest direction (attached to A)

Position-level Constraint

Use the dot-product form to avoid acos derivatives near 0 and π:

C = a_w . rest_w  -  cos(max_swing) >= 0

Violated when a_w . rest_w < cos(max_swing) (bone has swung outside the cone).

Derivation

C* = (a_w)* . rest_w + a_w . (rest_w)*
   = (wB x a_w) . rest_w + a_w . (wA x rest_w)

Apply stp:

(wB x a_w) . rest_w  =  wB . (a_w x rest_w)
a_w . (wA x rest_w)  =  wA . (rest_w x a_w)  =  -wA . (a_w x rest_w)

Combine and identify the swing axis axis = a_w x rest_w:

C* = (a_w x rest_w) . (wB - wA)
   = -axis . wA + axis . wB

Jacobian

J = [0, -axis, 0, axis]          axis = a_w x rest_w

Pure rotational.

Bias

Same sign rule as plane/limit (b = (beta/dt) * C):

b = (beta / dt) * ( a_w . rest_w - cos(max_swing) )      // negative when violated

Clamp

λ in [0, +inf)       // pushes bone back inside the cone

Effective Mass

A = axis . (IA^-1 + IB^-1) . axis

Impulse Application

wA <- wA - Δλ * IA^-1 . axis
wB <- wB + Δλ * IB^-1 . axis

Setup

static void swing_limit_setup(Setup *s, Row *row, v3 a_w, v3 rest_w, float max_swing)
{
      float c     = dot(a_w, rest_w);
      float c_max = cosf(max_swing);
      if (c >= c_max) return;                          // inside cone -- skip

      v3 axis = cross(a_w, rest_w);                     // swing axis
      float axis_len2 = dot(axis, axis);
      if (axis_len2 < 1e-8f) return;                    // degenerate (exactly aligned or anti-aligned)

      row->A = s->A;  row->B = s->B;
      row->JvA = (v3){0,0,0};
      row->JwA = neg(axis);
      row->JvB = (v3){0,0,0};
      row->JwB = axis;
      m3 I_sum = add(s->A->inv_I, s->B->inv_I);
      row->inv_A = 1.0f / dot(axis, mul(I_sum, axis));
      row->bias  = (s->beta / s->dt) * (c - c_max);     // negative when violated
      row->lo = 0.0f;
      row->hi = INFINITY;
      push_row(s, row);
}

Twist (angle limit)

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

Effective Mass and Impulse Application

Same as swing, with a_w in place of axis.

Setup

static void twist_limit_setup(Setup *s, Row *row, v3 a_w, float twist_angle, float twist_min, float twist_max)
{
      if (twist_angle < twist_min) {
              row->bias = (s->beta / s->dt) * (twist_angle - twist_min);
              row->lo   = 0.0f;
              row->hi   = INFINITY;
      } else if (twist_angle > twist_max) {
              row->bias = (s->beta / s->dt) * (twist_angle - twist_max);
              row->lo   = -INFINITY;
              row->hi   = 0.0f;
      } else {
              return;
      }
      row->A = s->A;  row->B = s->B;
      row->JvA = (v3){0,0,0};
      row->JwA = neg(a_w);
      row->JvB = (v3){0,0,0};
      row->JwB = a_w;
      m3 I_sum = add(s->A->inv_I, s->B->inv_I);
      row->inv_A = 1.0f / dot(a_w, mul(I_sum, a_w));
      push_row(s, row);
}

Typical ragdoll composition

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.

Warm Starting

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:

  1. 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.
  2. 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.
static void warm_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 is row_t0 last step. Just call warm_start_row on each of the joint's rows at the end of <joint>_setup:

static void prismatic_setup(PrismaticJoint *j, World *w, float beta, float dt)
{
      // ... 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 (each new contact c)
      for (each old contact o in the same manifold)
            if (c.feature_id == o.feature_id && c.feature_id != 0) {
                    c.lambda_n = o.lambda_n;
                    mark both as matched; 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 (each unmatched new contact c)
      find closest unmatched old contact o with |c.rA - o.rA|^2 < tolerance;
      if (o exists) 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.

float leftover = sum of lambda_n over unmatched old contacts;
float share   = leftover / (number of unmatched new contacts);
for each unmatched new contact: c.lambda_n = share;

Shallow-contact clamp

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:

if (c.penetration <= 0.0f) c.lambda_n = 0;       // speculative, don't warm-start
else {
      float scale = min(1.0f, c.penetration / (2.0f * SOLVER_SLOP));
      c.lambda_n *= scale;
}

Patch friction

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.

if (dot(old_rolling_axis, new_rolling_axis) > 0.5f) {
      new_lambda_roll = old_lambda_roll * dot(old, new);
} else {
      new_lambda_roll = 0.0f;
}

Storage

Two patterns for carrying last-step data forward:

  • 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:

static void warm_start_row(Row *row)
{
      if (row->accum == 0.0f) return;
      apply_impulse(row->A, row->B,
                    row->JvA, row->JwA, row->JvB, row->JwB,
                    row->accum);
}

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).

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment