Return trajectory early over threshold

[StarrryNight]

Description

This PR resolves #3591, which is a sub-issue of #3104.

The goal was to reduce unnecessary calculations in collision evaluator (previously getTrajectoryWithCost). These calculations are expensive, as it simulates the robot's position along the trajectory path at discrete time intervals (0.05s - 0.1s steps) and checks each position against all obstacles for collision detection. These operations, including getFirstNonCollisionTime(), getLastNonCollisionTime(), and getFirstCollisionTime(), are O(t × n) where t is the number of time steps and n is the number of obstacles.

By computing a running cost during evaluation and returning early when it exceeds the best known cost, we can skip:

• The backward scan for back collision duration (getLastNonCollisionTime)
• The most expensive mid-trajectory collision scan (getFirstCollisionTime)

This is especially effective because the front collision penalty has a 3× weight, so trajectories starting in obstacles quickly exceed the cost threshold and bail out after just the first check.

Because the primary goal of this PR is to optimize, the final trajectory selection of the optimized method is the same as the old algorithm, this is also tested later.

Results

This change was evaluated in two ways.

First, I logged the early returns in evaluations.
Screencast from 2026-02-01 21-37-44.webm
As you can see in the video, there were a lot of early returns. This means many trajectories exceeded the cost threshold before the full simulation completed, allowing us to skip the remaining expensive calculations. This optimization significantly reduces CPU usage since we no longer waste computation on trajectories that would ultimately be rejected anyway.

To quantify the improvement, I benchmarked CPU usage between the optimized version and Unix v1.1.0 (baseline) by screenrecording thunderscope and system monitor, and plotting the results onto a graph. Both versions were tested under identical conditions, with blue/yellow teams swapped to ensure equivalency. Results below:

The optimized versions shows notable reduction in cpu usage. Video recordings are below:

Blue optimized, yellow old:
Screencast from 2026-02-01 13-14-51.webm

Blue old, yellow optimized:
Screencast from 2026-02-01 13-32-43.webm

Testing Done

1. Same Trajectory Outcome
The optimization must not change which trajectory is selected. This is guaranteed by the following invariant:
The best trajectory always has its full cost calculated. it is never the result of an early return from the collision evaluator.

To test this, I simply kept the calculateCost function, and logged the boolean of:
best_traj.cost == calculateCost(best_traj)
where calculate cost returns the full calculation.

This test passed, as in thunderscope logging, only 1s (True) were logged.

2. Trajectory Correctness
This was simply tested by running trajectory planner tests. All test cases passed.

3. It is actually returning early
This was tested in the results section by logging early returns.

Resolved Issues

resolves #3591

Length Justification and Key Files to Review

Review Checklist

It is the reviewers responsibility to also make sure every item here has been covered

• Function & Class comments: All function definitions (usually in the .h file) should have a javadoc style comment at the start of them. For examples, see the functions defined in thunderbots/software/geom. Similarly, all classes should have an associated Javadoc comment explaining the purpose of the class.
• Remove all commented out code
• Remove extra print statements: for example, those just used for testing
• Resolve all TODO's: All TODO (or similar) statements should either be completed or associated with a github issue

[williamckha]

Awesome work, thanks for this change and the very detailed analysis.

[annieisawesome2]

Looks good! Very clean and readable!

[annieisawesome2]

You use 6.0 later. Should we keep it consistent and make this 3.0 (since we are using doubles)?

[nycrat]

Added a few comments about max_cost being an std::optional

The logic for returning early and performance benefits seem good from your screen recordings and testing

[StarrryNight]

@nycrat i just converted it into a plain double I think that is more suitable.