perf(fclrave): visit only changed bodies in Synchronize and EnsureBodies

[ziyan]

Problem

FCLCollisionChecker::_GetEnvManager calls FCLCollisionManagerInstance::EnsureBodies and Synchronize on every environment-level CheckCollision, and both walked every cached body regardless of whether anything moved. The cost tracked environment size rather than amount of change: in a 200-body benchmark, moving nothing cost 6.9us per check and moving all 200 cost 6.5us.

Change

FCLSpace keeps a change log. _MarkBodyChanged records a body against a monotonic revision counter, CollectBodyIndicesChangedAfter returns the bodies recorded since a caller's last sync, and each manager keeps a cursor so it revisits only those.

The mark is set inside FCLSpace::_Synchronize, where an info is actually refreshed, rather than at the point of mutation. KinBody::Link::SetTransform, KinBody::IncrementUpdateStamp and GetNonAdjacentLinks bump the body update stamp without going through _PostprocessChangedParameters, so no change callback fires for them. Marking at the point of refresh covers those, and FCLSpace::Synchronize deliberately stays a full scan for the same reason -- its per-body cost is one integer compare.

Commit 2 stores the log in a vector sorted by revision rather than a std::map. Revisions only increase, so marking appends and a query binary searches once then walks contiguous memory; superseded entries are skipped on read and dropped by a compaction pass.

Commit 3 hardens the bookkeeping: marks are recorded before the mutation they describe, the manager cursor advances only after a whole Synchronize succeeded, teardown in RemoveUserData/DestroyEnvironment is indexed off the slot the body was initialized under, and attached bodies skipped because the space had not initialized them yet are retried.

Commit 4 fixes the active-DOF refresh caching a link BV it had just unregistered; the next refresh then unregistered (or replaceObjectd) an object the broadphase manager does not hold.

Commit 5 applies the change log to EnsureBodies with its own cursor, separate from Synchronize's so both consumers see the same marks. Recovery for a body the space stopped tracking is preserved: re-initializing it records a new mark in InitKinBody, which the cursor picks up. A valid cache entry holding a different body than the one now at that index is dropped and re-added instead of skipped.

Results

Benchmark, 190 checks/cycle over 200 bodies: ~6.5us -> ~1.1us per check (~2us after commits 1-2, ~1.1us after commit 5), and the cost now scales with how many bodies moved instead of being flat.

Commit 5 measured on its own, building the tree at commit 4 and at commit 5 and running the same benchmark:

bodies moved	before commit 5	after commit 5	change
0	1.86us	1.07us	-42%
1	1.61us	1.20us	-25%
10	1.58us	1.10us	-30%
50	1.86us	1.18us	-37%
100	1.61us	1.06us	-34%
200	2.03us	1.13us	-44%

Those are the minimum of several runs per configuration. The benchmark host is shared and noisy: individual runs of the same build vary by up to 2x, and the first run after a build is consistently slow from cold caches, so single numbers are not meaningful. The minimum is the honest estimator here because contention only ever adds time. What carries the conclusion is the floor rather than any single row: after commit 5 every row reached 1.06-1.20us in its best run, while before it never went below 1.58us on any row across three runs.

Profiled on a production fleet controller, share of the scheduling thread:

	before	after commit 1	after commit 2
FCLCollisionManagerInstance::Synchronize	46.5%	15.4%	11.2%
CollectBodyIndicesChangedAfter	-	5.2%	1.2%

EnsureBodies was a further 8.2% of the same thread before commit 5.

Testing

Re-run on the final tree unless noted:

• A randomized workload driving an articulated scene through SetDOFValues, Link::SetTransform, grab/release, active-DOF tracking, geometry group switches, link and body enabling, and adding and removing bodies mid-run. Each iteration answers the same query with the long-lived cached checker and with a checker created that instant; any disagreement is a stale cache. 17,598 comparisons, zero mismatches, and zero FCL unregisterObject warnings (commit 4 removes the 12 an earlier revision produced). An earlier revision that marked via a Prop_LinkTransforms callback fails it with 22 mismatches, so the check is sensitive to exactly this class of bug.
• A deterministic 20,000-iteration collision checksum: identical to an unpatched build.
• test/test_collision.py fcl suite: unchanged before and after.
• A downstream collision test suite of 578 tests: identical pass/fail sets before and after.
• Full integration run pinned to the first two commits: 1392 pass, 5 fail, all 5 reproduced on an unpatched baseline.