Transfer collision detection optimizations from arjo129/mapf_post

[uday-kalyan-s]

New feature implementation

Implemented feature

Adds an AABB sweep-and-prune broad phase to mapf::post::mapf_post's Type‑2
(cross-agent collision) dependency detection, replacing the previous
brute-force O(N²T²) nested-loop check. Also adds Criterion benchmarks so the
improvement (and any future regression) is measurable rather than just
asserted.

Implementation description

The broad phase mirrors the approach already used in the sibling mapf_post
crate: for every trajectory segment, compute a merged AABB spanning its
start and end pose, pick whichever axis (X or Y) has the larger spread
across all segments, sort segments by AABB min on that axis, then sweep with
an early-exit break once a later segment's AABB min passes the current
segment's AABB max on that axis. Only AABB-overlapping pairs go through the
exact continuous collision check (collides, via
parry2d::query::cast_shapes_nonlinear) that used to run on every pair
unconditionally.

Changes:

• mapf/mapf/src/post/mod.rs: replaced the brute-force Type‑2 edge loop in
  mapf_post with the sweep-and-prune broad phase.
• mapf/mapf/src/post/mod.rs (sweep_line_tests module): added tests
  comparing the sweep-line output against a brute-force reference
  implementation across several scenarios (crossing, following, head-on
  swap, diagonal, far-apart, unequal-length trajectories), plus a
  sort-axis-invariance test (X-sort vs. Y-sort scenes must agree).

Complexity is unchanged in the worst case (still O((N·T)²) when a scene is
AABB-dense on both axes), but substantially better in the common case of
spatially separated trajectories, since the sweep prunes the AABB
comparisons that no longer need the expensive narrow-phase check.

GenAI Use

We follow OSRA's policy on GenAI tools

• I used a GenAI tool in this PR.
• I did not use GenAI

Assisted by: Claude Sonnet 5

[uday-kalyan-s]

@arjo129 I believe we are sticking to the sweep line solution. I think this one is fairly good because we are doing it across the axis with maximum spread. Should I try experimenting with integrating the AABB tree approach I mentioned? I think we can still get a better performance

[arjo129]

Sure!

[uday-kalyan-s]

Heyy @arjo129 . AABB tree performs kind of poorly with robots under 5k robots. there is a 1.5x improvement with the tree only after 16k robots. I think it is probably due to the data structure construction and indexing itself being time consuming compared to a simple sweep line sort. Given that practically the numbers should be much lesser, can we just merge the PR with the sweep line solution?

[arjo129]

Yes. I'd recommend going for the sweep line solution. But if you have an implementation of the AABb being worse it'd be awesome to share it.

[uday-kalyan-s]

Sure I do. I'll just clean up the code and push it here in a while.

[uday-kalyan-s]

Heyy @arjo129. I have a tiny update. So I tested previously with my own implementation of AABB tree which led to it being slower. Testing it with parry2d crate's implementation works better though. They have simd optimizations.
cargo test --release -p mapf post::timing::print_grid_scene_timings -- --ignored --nocapture is the command used to test it out. there are the results right now

    agents          sweep      aabb_tree
       100      852.579µs      648.739µs
       500     3.021628ms     3.274097ms
      1000     7.467598ms     5.773301ms
      2000    18.649675ms    13.729079ms
      4000    49.207466ms    30.466066ms
      8000   109.230276ms    58.391738ms
     16000   303.399525ms    123.49337ms
     32000   872.076826ms    328.71183ms

Can we move with the AABB tree implementaton then?

[uday-kalyan-s]

I will just try to fix the style check for now

[arjo129]

1,10,50 are important benches and then also trajectory lengths are important.

[uday-kalyan-s]

Ohkk yes il add that too

[arjo129]

Also for the genAI tool, we are required to use the Assisted by: attribution, so it'd be good if you just filled in which genAI tool you used.

[uday-kalyan-s]

Yepp just added the attribution. thanks for reminding

[uday-kalyan-s]

== sweep: agents (rows) x trajectory length (columns) ==
    agents          len=1         len=10         len=50
         1        7.325µs        7.079µs       38.891µs
        10         2.73µs         74.2µs      552.406µs
        50        17.07µs      580.967µs     4.943438ms
       100       35.291µs      818.497µs     4.867423ms
       500       56.075µs     3.442177ms    51.242402ms
      1000      121.618µs     6.589219ms   124.768928ms
      2000      282.022µs    16.915353ms    334.44241ms
      4000      425.711µs    37.990069ms   904.069338ms
      8000      887.813µs   102.260953ms   2.403690385s
     16000     1.920682ms   290.949257ms   6.495021897s
     32000     4.140242ms   745.636194ms  18.203365929s

== aabb_tree: agents (rows) x trajectory length (columns) ==
    agents          len=1         len=10         len=50
         1        2.027µs        9.056µs        58.67µs
        10         3.24µs       55.324µs       503.81µs
        50        6.615µs      261.706µs     2.177994ms
       100       11.018µs      505.217µs     4.239582ms
       500       55.652µs     2.464262ms    22.593234ms
      1000      112.252µs      4.90578ms    46.691776ms
      2000      226.753µs    11.828848ms   101.401416ms
      4000      444.005µs    24.646234ms   228.952406ms
      8000      898.877µs    55.279793ms   524.868159ms
     16000     2.036268ms   109.343766ms    1.08999791s
     32000     4.763935ms   311.433841ms   2.503588567s

These are the results for robots against path length. I kept path length kind of small but i think its O(NT) which matters so it should scale accordingly.

[uday-kalyan-s]

Is the implementation and testing fine now?

[arjo129]

Could you check the benches in as well so I can replicate it on my end??

[uday-kalyan-s]

The results can be gotten through the command
cargo test --release -p mapf post::timing::print_grid_scene_timings -- --ignored --nocapture which prints it to the console . there is just one by default hidden test which can be manually turned on to log the timings. Should I make it output to a file? The tests have already been committed and pushed and are in src/post/mod.rs

[arjo129]

Ah got it. I'll take a look. I dont think that we should be using test for this, but we should be using cargo bench

[uday-kalyan-s]

Ohhh I see. Thanks, il switch over to cargo bench then.

[uday-kalyan-s]

Hi. Sorry for the delay. @arjo129 You can try testing it with cargo bench now