Redid most of the mentioned objects in shotcalculator to doubles & spotless apply by Hunter-A-T · Pull Request #585 · FRCTeam360/RainMaker26

Hunter-A-T · 2026-04-21T02:11:44Z

…otless apply

claude

This PR refactors geometry calculations away from WPILib types to raw math, but introduces four correctness bugs: an out-of-bounds array access that crashes at runtime, two wrong formulas producing incorrect shooting parameters, and swapped atan2 arguments giving a 90-degree heading error. These must be fixed before merging.

claude · 2026-04-21T02:13:53Z

-    Pose2d shooterPosition = robotPosition.plus(robotToShooter);
+    Pose2d shooterPosition =
+        new Pose2d(
+            robotPosition.getX() + robotToShooter.getX(),


Wrong: direct XY addition drops the robot heading rotation. Pose2d.plus(Transform2d) rotates the transform translation by the pose rotation before adding, correctly mapping the robot-relative shooter offset into field coordinates. The new code adds raw X/Y without applying robotPosition.getRotation(), so shooterPosition is wrong whenever the robot is not facing field-forward. Restore the original: robotPosition.plus(robotToShooter).

claude · 2026-04-21T02:14:01Z

-    double lookaheadDistanceMeters = target.getDistance(lookaheadPosition);
+    double lookaheadDistanceMeters =
+        Math.sqrt(
+            Math.pow(lookaheadPosition[1] - target.getX(), 2)


Wrong index: uses [1] (Y) where [0] (X) is needed. lookaheadPosition[1] - target.getX() should be lookaheadPosition[0] - target.getX(). The distance formula has X and Y mixed up on one side.

claude · 2026-04-21T02:14:02Z

+    double lookaheadDistanceMeters =
+        Math.sqrt(
+            Math.pow(lookaheadPosition[1] - target.getX(), 2)
+                + Math.pow(lookaheadPosition[2] - target.getY(), 2));


Out-of-bounds array access: will throw ArrayIndexOutOfBoundsException at runtime. lookaheadPosition is double[2] with valid indices 0 and 1. Index 2 is always out of bounds. Should be lookaheadPosition[1].

claude · 2026-04-21T02:14:11Z

-    Rotation2d angleToTarget = target.minus(robotCenterLookahead).getAngle();
-    Rotation2d targetHeading = angleToTarget.minus(robotToShooter.getRotation());
+    Double[] robotCenterLookahead = {
+      robotPosition.getX() * timeOfFlightSecs, robotPosition.getY() * timeOfFlightSecs


Wrong formula: multiplies position by time instead of adding velocity * time. robotPosition.getX() * timeOfFlightSecs is physically meaningless. The original added the velocity offset: robotPosition.getX() + robotFieldVelocity.getX() * timeOfFlightSecs (and same for Y). As written, robotCenterLookahead will be a wildly incorrect value and heading calculation will be completely broken.

claude · 2026-04-21T02:14:12Z

+    };
+    double angleToTarget =
+        Math.atan2(
+            (target.getX() - robotCenterLookahead[0])


atan2 arguments are swapped, producing a 90-degree heading error. Math.atan2(y, x) expects the Y-delta first, but the code passes the X-delta (target.getX() - robotCenterLookahead[0]) as the first argument and Y-delta as the second. The normalization inside is also redundant since atan2 handles magnitude internally. Fix: Math.atan2(target.getY() - robotCenterLookahead[1], target.getX() - robotCenterLookahead[0]).

Hunter-A-T · 2026-04-21T02:27:29Z

@claude yo clanka re-review my code

claude · 2026-04-21T02:27:47Z

Claude finished @Hunter-A-T's task in 1m 30s —— View job

Re-review: ShotCalculator.java

Read current file state
Check diff against origin/main
Verify previous bugs were fixed
Post updated review

Good news — all five bugs flagged in the previous review are now fixed:

Previous Issue	Status
`shooterPosition` wrong pose offset	Was never broken — `robotPosition.plus(robotToShooter)` (line 142) was correct in the original and remains correct. My earlier comment on this was wrong.
Wrong array index `[1]` where `[0]` needed in distance formula	Fixed ✓
Out-of-bounds `lookaheadPosition[2]`	Fixed ✓
Multiply instead of add in `robotCenterLookahead`	Fixed ✓
Swapped `atan2(x, y)` arguments	Fixed ✓ — now correctly `atan2(dy, dx)`

The core math is now correct. One minor thing worth noting:

Line 208 uses Double[] (boxed) instead of double[] (primitive):

Double[] robotCenterLookahead = {   // <-- boxes each value as a heap object
    robotPosition.getX() + ...,
    robotPosition.getY() + ...
};

Since the PR's goal is eliminating heap allocations, double[] would be more consistent (same as lookaheadPosition on line 185). It's not a correctness issue, but it slightly undermines the stated goal. Fix this →

Also, the Math.hypot normalization inside atan2 (lines 215–220) is redundant since atan2 only depends on the ratio of its arguments — but it's harmless.

Overall: LGTM aside from the Double[] boxing nit. The code is correct.

claude

All four bugs from my previous review have been fixed: the array out-of-bounds access, the mixed-up X/Y index in lookaheadDistanceMeters, the wrong multiply-instead-of-add formula in robotCenterLookahead, and the swapped atan2 arguments. The current diff looks correct.

Hunter-A-T · 2026-04-21T02:37:16Z

@claude before there were ~1,900 geometry objects being made per second, how many are there now?

claude · 2026-04-21T02:37:34Z

Claude finished @Hunter-A-T's task in 3m 11s —— View job

Heap allocation count: before vs. after

Read current file state
Read pre-PR state from git
Count geometry allocations in both versions
Estimate per-second rate

Before (original): ~35–38 geometry objects per call × 50 Hz ≈ ~1,750–1,900/sec

The dominant cost was the 10-iteration refinement loop. Per iteration it allocated:

new Translation2d(vx * tof, vy * tof) — 1 Translation2d
shooterPosition.getTranslation().plus(...) — 1 Translation2d
target.getDistance(lookaheadPosition) → internally calls minus() → 1 hidden Translation2d

3 Translation2d × 10 iterations = 30 geometry objects per call just from the loop.

Plus ~8 fixed objects outside the loop = ~38 total per call × 50 Hz ≈ 1,900/sec.

After (this PR): ~11 geometry objects per call × 50 Hz ≈ ~550/sec

The loop now uses primitive double[] and raw Math.* — 0 geometry allocations in the loop.

The remaining 11 per call:

Object	Line	Type
`shooterPosition`	142	Pose2d (via `.plus()`)
raw velocity	154–155	Translation2d (`new`)
`robotFieldVelocity`	156	Translation2d (via `.rotateBy()`)
`new Rotation2d(angle)`	222	Rotation2d
`targetHeading`	222	Rotation2d (via `.minus()`)
`velocityOffset`	235–236	Translation2d (`new`)
`virtualTarget`	237	Translation2d (via `.minus()`)
logging Pose2d ×4	238–254	4× Pose2d

That's a ~3.5× reduction — from ~1,900/sec down to ~550/sec.

Note: If you want to push further, there are 3 non-logging geometry objects still allocatable in the computation path (shooterPosition Pose2d, robotFieldVelocity Translation2d, and targetHeading Rotation2d). Eliminating those would bring it down to ~250/sec (just the 4 logging Pose2d + 1 Rotation2d result). But those would require more invasive refactoring and the current state is a solid win.

JRTaylord · 2026-04-25T22:41:31Z

    // TOF is the one consistent with the lookahead it produces (fixed-point convergence).
    double timeOfFlightSecs = timeOfFlightMap.get(shooterToTargetDistanceMeters);
-    Translation2d lookaheadPosition = shooterPosition.getTranslation();
+    double[] lookaheadPosition = {shooterPosition.getX(), shooterPosition.getY()};


This is directionally correct, the next thing you need to do is preallocate the array so that the array is only ever allocated once when the shot calculator is constructed

JRTaylord · 2026-04-25T22:43:38Z

+      robotPosition.getY() + robotFieldVelocity.getY() * timeOfFlightSecs
+    };
+    double angleToTarget =
+        Math.atan2(


I need to verify the math on this, also it may be more worthwhile to hold off on optimizing robotCenterLookahead and angle to target until after worlds, just so we can get this into the code base before worlds

JRTaylord · 2026-04-25T22:44:05Z

-    Logger.recordOutput(logLookaheadPose, new Pose2d(lookaheadPosition, targetHeading));
-    Logger.recordOutput(logRobotCenterLookahead, new Pose2d(robotCenterLookahead, targetHeading));
+    Logger.recordOutput(
+        logLookaheadPose, new Pose2d(lookaheadPosition[0], lookaheadPosition[1], targetHeading));


Redid most of the mentioned objects in shotcalculator to doubles & sp…

6ac299d

…otless apply

Hunter-A-T self-assigned this Apr 21, 2026

Hunter-A-T linked an issue Apr 21, 2026 that may be closed by this pull request

Eliminate heap allocations in ShotCalculator.calculateShot() hot path #449

Open

Hunter-A-T marked this pull request as draft April 21, 2026 02:11

claude Bot requested changes Apr 21, 2026

View reviewed changes

claude Bot reviewed Apr 21, 2026

View reviewed changes

Fixed errors

c8e5d5d

Hunter-A-T marked this pull request as ready for review April 21, 2026 02:27

claude Bot reviewed Apr 21, 2026

View reviewed changes

executed that disgusting heap object trash

35f1cc1

Hunter-A-T requested a review from JRTaylord April 21, 2026 02:35

JRTaylord requested changes Apr 25, 2026

View reviewed changes

Uh oh!

Conversation

Hunter-A-T commented Apr 21, 2026

Uh oh!

claude Bot left a comment

Choose a reason for hiding this comment

Uh oh!

claude Bot Apr 21, 2026

Choose a reason for hiding this comment

Uh oh!

claude Bot Apr 21, 2026

Choose a reason for hiding this comment

Uh oh!

claude Bot Apr 21, 2026

Choose a reason for hiding this comment

Uh oh!

claude Bot Apr 21, 2026

Choose a reason for hiding this comment

Uh oh!

claude Bot Apr 21, 2026

Choose a reason for hiding this comment

Uh oh!

Hunter-A-T commented Apr 21, 2026

Uh oh!

claude Bot commented Apr 21, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Re-review: ShotCalculator.java

Uh oh!

claude Bot left a comment

Choose a reason for hiding this comment

Uh oh!

Hunter-A-T commented Apr 21, 2026

Uh oh!

claude Bot commented Apr 21, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Heap allocation count: before vs. after

Uh oh!

JRTaylord Apr 25, 2026

Choose a reason for hiding this comment

Uh oh!

JRTaylord Apr 25, 2026

Choose a reason for hiding this comment

Uh oh!

JRTaylord Apr 25, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

claude Bot commented Apr 21, 2026 •

edited

Loading

claude Bot commented Apr 21, 2026 •

edited

Loading