Objective

Our current solver is highly bottlenecked by expensive ECS queries and sub-optimal cache performance. Each constraint currently looks up RigidBodyQueryItem structs for the involved bodies, which holds 16 different properties/components, many of which are optional. This results in a ton of overhead in a very hot loop.

We need a more efficient and densely packed representation for body data required by the solver. This will also make vectorization and eventual SIMD constraints much more viable to implement.

Solution

Add SolverBody and SolverBodyInertia structs designed to improve memory locality and performance. They are initialized with body data before the solver, and the results are written back after the solver. In 2D, the types look like this:

// This representation is inspired by b2BodyState in Box2D v3.
// 32 bytes in total
#[cfg(feature = "2d")]
pub struct SolverBody {
    pub linear_velocity: Vec2, // 8 bytes
    pub angular_velocity: f32, // 4 bytes
    pub delta_position: Vec2,  // 8 bytes
    pub delta_rotation: Rot2,  // 8 bytes
    pub flags: u32,            // 4 bytes (currently unused)
}

// This representation is my own current design :D
// 16 bytes in total
#[cfg(feature = "2d")]
pub struct SolverBodyInertia {
    // Includes locked axes
    effective_inv_mass: Vec2,   // 8 bytes
    effective_inv_inertia: f32, // 4 bytes
    flags: InertiaFlags,        // 4 bytes
}

Only awake dynamic bodies and kinematic bodies have an associated solver body. Static bodies and sleeping dynamic bodies do not move, so they instead use a "dummy state" with SolverBody::default(). However, this raises a problem: if the solver doesn't have access to the position or rotation of static bodies, how can we compute constraint anchors? There's two options:

• Option 1: Use delta positions and rotations. This requires preparing base anchors and other necessary positional data in world space, and computing the updated anchors during substeps.
• Option 2: Use full positions and rotations. This requires storing anchors in world space for static bodies and sleeping bodies, and in local space for dynamic bodies.

Avian uses Option 1, because:

• Using delta positions reduces round-off error when bodies are far from the origin.
• Mixing world space and local space values depending on the body type would be quite confusing and error-prone, and would possibly require more branching.

As a result, joints now also have a prepare step before the substepping loop where they prepare some base offsets for the solver.

This approach was largely inspired by Box2D v3, with some small modifications, like how inertial properties are stored. Box2D stores them in constraints, which leads to duplicating some data, whereas I store them per-body.

Storing Solver Bodies

For storing these solver bodies, I investigated two approaches:

• Option 1: Maintain a SolverBodies resource with Vecs for the body data, and use a SolverBodyIndex component to map rigid body entities to them.
• Option 2: Store SolverBody as a component on the rigid body entity itself.

Option 1 gives us a bit more control and ends up being slightly faster especially for random access, since it's just indexing a Vec. However, Option 2 has way less complexity and bookkeeping, and integrates better with the ECS (ex: no need for double-lookups when both ECS data and the solver body is needed, easier parallel iteration, integration with query filters...), so I opted to use that for the initial PR. We will also benefit from any improvements to Bevy's Query::get performance here.

You can see how the two approaches differ in the Results section at the end.

Changes to Constraints

As mentioned before, joints now need a prepare step to prepare base translational and rotational offsets and the relative dominance of the bodies (we may be able to abstract dominance out somehow). This is done once per timestep, before substepping. Joints then compute updated offsets and other positional data based on the pre-computed base data and the delta_position and delta_rotation.

Contact constraints have undergone similar changes, but we can now also get rid of the local_anchor1 and local_anchor2, as we can now apply the delta_rotation to the world-space anchor1 and anchor2.

We also have way less branching, and don't check whether bodies are dynamic or not before applying constraint impulses. If they aren't, the mass will be infinite anyway, and the impulse won't do anything.

Joint Refactor

Aside from changes required for solver bodies, I have also refactored joints in many other ways.

• The entities method now uses a separate EntityConstraint type to allow reusable constraints that don't store entities.
• Added a PointConstraint and FixedAngleConstraint to share more constraint logic between different joint types.
• Properties that aren't meant to be written to by users are now private and have read-only getters.
• Many methods such as with_compliance have been moved out of the Joint trait.
• Each part of a constraint has its own compliance and helpers for it, such as with_point_compliance and with_swing_compliance.
• xpbd::solve_constraint is now xpbd::solve_xpbd_joint, and only supports two entities (make your own system if you need more).
• Many more internal refactors.

Other Notable Changes

The following components have been removed and are no longer stored for rigid bodies:

• AccumulatedTranslation
• PreSolveAccumulatedTranslation
• PreSolveLinearVelocity
• PreSolveAngularVelocity
• PreSolveRotation
• PreviousRotation

That's a lot of unnecessary data gone! Some of them were already redundant, and others were made unnecessary by the changes in this PR.

Instead, we now require two new components:

• PreSolveDeltaPosition
• PreSolveDeltaRotation

These are needed for XPBD velocity projection, but can be removed once we switch joints to be impulse-based.

There are frankly so many small internal changes that I can't really cover them all in detail, so I suggest looking at the diff for more details. For the most part, the changes are internal, and won't directly affect the average user (beyond the improved performance :D).

Results

The results are quite remarkable. In the pyramid_2d example, using a base of 100 boxes, the results look like the following with 6 substeps and the parallel feature enabled.

• Left: main branch, no solver bodies.
• Middle: Solver bodies stored in a SolverBodies resource, mapping entities to them with a SolverBodyIndex component.
• Right: Solver bodies stored directly on the entities with a SolverBody component.

That is nearly a 3x performance improvement in this collision-heavy scene. Crazy!

Using SolverBody as a component is slightly more expensive here, but for now, I think it's better to start with that to reduce complexity. We can always re-assess this later.

Future Work

There's still a lot more we can do to optimize things further. Some ideas include:

• Optimize velocity integration to pre-compute velocity increments before substepping. Rapier does this.
• Make gyroscopic torque opt-in.
• Figure out if there's a better layout for the 3D SolverBody.
• Change 3D angular inertia to use a custom SymmetricMat3 type (I have this mostly implemented).
• Implement graph coloring and SIMD constraints. The SolverBody layout is designed to be quite optimal for this, at least in 2D.

Migration Guide

Avian's solver now uses special SolverBody and SolverBodyInertia components instead of separate components like Position, Rotation, AccumulatedTranslation, LinearVelocity, and AngularVelocity. They are initialized for awake dynamic and kinematic bodies in the substepping loop, and the results are written back to the user-facing components at the end. This also means that if you are running custom logic inside the substepping loop, you should generally use SolverBody instead of the separate components.

Solver bodies were added to drastically improve the performance of the solver, but it also means significant changes to internals.

Removed Components and Properties

The following components have been removed and are no longer stored for rigid bodies:

• AccumulatedTranslation
• PreSolveAccumulatedTranslation (now called PreSolveDeltaPosition)
• PreSolveLinearVelocity
• PreSolveAngularVelocity
• PreSolveRotation
• PreviousRotation

A new PreSolveDeltaRotation component has also been added.

The current_position helper on RigidBodyQueryItem and ColliderQueryItem has been removed.

The local_anchor1 and local_anchor2 properties on ContactConstraintPoint have been removed.

Joints

• Properties that aren't meant to be written to by users are now private and have read-only getters.
• Many methods such as with_compliance have been moved out of the Joint trait.
• Each part of a constraint has its own compliance and helpers for it, such as with_point_compliance and with_swing_compliance.
• The entities method now uses a separate EntityConstraint trait to allow reusable constraints that don't store entities.
• The XpbdConstraint trait now has a prepare step to prepare base translational and rotational offsets and any other pre-step data.
• XpbdConstraint::solve and many related methods now use SolverBody and SolverBodyInertia structs. Positional corrections are applied to the delta_position and delta_rotation properties. Updated positional information can be computed based on the pre-step data and these deltas.
• Many joints store a new PointConstraint and FixedAngleConstraint to share more constraint logic between different joint types.
• xpbd::solve_constraint is now xpbd::solve_xpbd_joint, and only supports two entities (make your own system if you need more).
• Many more internal refactors.

Other

• SolverSet::ApplyTranslation has been renamed to SolverSet::Finalize.