Add KD-tree conflict detection benchmark slice

[ArizmendiWan]

This PR adds a first slice for evaluating faster conflict detection in mapf negotiation by introducing an experimental KD-tree broad phase for proposal conflict checks.

What Changed

• add ConflictDetectionAlgorithm::{Baseline, KdTree} and detect_conflicts_for_proposals(...)
• add a KD-tree broad phase over trajectory bounding-box envelopes before exact conflict checks
• add parity tests to compare baseline vs KD-tree outputs on deterministic synthetic workloads
• add a Criterion benchmark for low/medium/high-overlap synthetic cases
• add a small example that prints baseline-vs-KD-tree comparison stats
• add notes describing the benchmark slice and how to run it

Why

This is meant to test whether a spatial broad phase can reduce candidate pair enumeration before exact conflict checking, especially in sparse workloads, without changing planner semantics.

Verification

Run locally with:

CARGO_HOME=$PWD/.cargo_home cargo test -p mapf
CARGO_HOME=$PWD/.cargo_home cargo run --release -p mapf --example conflict_detection_report
CARGO_HOME=$PWD/.cargo_home cargo bench -p mapf --bench conflict_detection

Observed comparison from the report example:

• low overlap, 64 agents, 32 waypoints: pair enumeration 2016 -> 363
• runtime ~0.51 ms -> ~0.05 ms
• conflict outputs matched the baseline path in the added parity tests

GenAI Use

• I used a GenAI tool in this PR.
• I did not use GenAI
  Generated-by:
  OpenAI Codex (v0.115.0) with GPT-5.4

[arjo129]

This is somewhat interesting. However, as Dijkstra said pre-mature optimization is the root of all evil. I opened #41 out of curiosity to see if such a kd-tree collision check is actually valueable here. I took the collision checking code and integrated it with the negotiation code. I then ran benchmarks on both branches and had gemini write a small summary of the resultant data.

TBH, from what I see, there are really small gains from using such a method. I suspect the bottleneck is elsewhere - that being said this is still a welcome addition. Perhaps a lower hanging fruit would be to switch from optimal search during the conflict negotiation to Explicit-Estimation Search or Focal Search.

[ArizmendiWan]

This is somewhat interesting. However, as Dijkstra said pre-mature optimization is the root of all evil. I opened #41 out of curiosity to see if such a kd-tree collision check is actually valueable here. I took the collision checking code and integrated it with the negotiation code. I then ran benchmarks on both branches and had gemini write a small summary of the resultant data.

TBH, from what I see I really small gains from using such a method. I suspect the bottleneck is elsewhere - that being said this is still a welcome addition. Perhaps a lower hanging fruit would be to switch from optimal search during the conflict negotiation to Explicit-Estimation Search or Focal Search.

Thank you for your feedbacks!

I reproduced your results from the benchmark branch and also ran additional experiments with focal search. I think your conclusion is correct, conflict detection is not the main bottleneck in many scenarios.

From my experiments, more than 70% of the latency comes from high-level negotiation and low-level replanning, which also caused some cases to timeout. I also tested focal search. It helps in some cases, but it is not robust enough by itself. For example, it still fails on cases such as empty-32-32:S1:a5, room-32-32-4:S10:a20, and several other difficult instances.

I plan to focus next on reducing the number of replans or making each replan cheaper.