Swept-volume collision checking between configurations

[Onwcan]

What is missing

CollisionModel::clearance answers a question about one configuration.

Nothing checks the volume swept between two configurations, so a step large enough to pass through a thin obstacle can pass through it undetected.

The obligation to sample densely enough currently sits with the caller. A caller who does not know that can receive a clean report for a move that actually passed through an obstacle.

Why it matters

Every other failure mode in this library announces itself. This one does not.

A result such as to_obstacles = 0.31 m can be correct for the configurations that were asked about and still be useless for the motion between them.

A per-configuration checker used as a gate on setpoints, which ADR-0015 explicitly encourages because the existing check is cheap enough at approximately 432 ns, is exactly where a single large step is most likely to expose this gap.

Proposed approach

Option 1 — Conservative bound

Recommended first.

For a step q0 -> q1, bound how far any point on any capsule can travel.

For each link, use the sum over its ancestor joints of |dq_j| * r_j, where r_j is the distance from joint j's axis to the farthest point of that link.

If the clearance at both endpoints exceeds that bound, the swept volume is provably clear.

This requires no subdivision, no new geometry, and only one pass over the chain.

It may refuse some safe moves, which is the correct direction to be wrong in.

Option 2 — Adaptive subdivision

Bisect the step until the conservative bound is satisfied on every sub-step or a depth cap is reached.

The API could then return the fraction of the step that is provably clear, allowing callers to truncate rather than simply refuse a move.

Option 1 alone closes the silent-failure hole. Option 2 is an additional convenience.

Where

• include/motionkit/core/collision.hpp — Clearance, CollisionModel
• src/core/collision.cpp

Acceptance criteria

• Add a sweptClearance(q0, q1) API, or equivalent, returning at minimum clear / not-provably-clear plus the conservative bound that was used.
• Add a regression test using a thin obstacle and a step that jumps across it.
• In that regression test, the existing per-configuration checks must report clear at both endpoints.
• The swept-volume check must refuse the same move.
• Add a test over a few hundred random step pairs proving that the conservative bound never reports clear when dense sampling finds a collision.
• Keep the implementation allocation-free.
• Keep the implementation non-throwing.
• Benchmark the new check and publish its cost next to the existing 432 ns figure.
• Amend ADR-0015, whose consequence section currently records this as a known limitation.

References

docs/adr/0015-collision-checking-in-capsules.md, Consequences bullet 2.