Raycasting

[Okim29]

No description provided.

[claude]

The canPass() logic is incomplete -- it always returns true regardless of hub obstruction, so callers will get incorrect results. There is also test-only code left in periodic() that runs every cycle and should be removed before merging.

[claude]

This test invocation of canPass() runs every cycle in periodic() and the comment explicitly marks it for deletion. It performs a full raycast including a Logger.recordOutput call inside a loop and discards the result. Remove this before merging.

[claude]

canPass is initialized to true on line 150 and is never set to false. The hub intersection is computed and visualized but never used to gate the return value, so this method always returns true regardless of whether the shot path is blocked. The end point returned by findIntersectionWithHub needs to be checked -- if it differs from the original field-edge end (an intersection was found), canPass should be set to false.

[Okim29]

Haven't added logic for if we can actually pass yet but I have logic for the raycast

[Okim29]

@claude review my changes for performance in terms of how many objects are allocated per cycle

[claude]

Claude finished @Okim29's task in 1m 12s —— View job

---

Performance Review: Object Allocation per Cycle

• Read changed files
• Analyze object allocations in hot paths
• Post findings

---

Summary

canPass() is called every cycle from periodic() and has significant allocation pressure — primarily from a per-meter sampling loop inside findIntersectionWithHub. Here's the full breakdown:

---

Allocations per call to canPass() (called every cycle)

Location	Object	Count
periodic() line 313: robotPose.getRotation().plus(...)	Rotation2d	1
canPass() line 151: robotPose.getTranslation()	Translation2d	1
canPass() line 165: new Translation2d(maxDistance, shooterRotation)	Translation2d	1
canPass() line 165: start.plus(...)	Translation2d	1
findIntersectionWithHub() line 179: new Translation2d(x, y) inside loop	Translation2d	N per cycle
canPass() line 168: new Pose2d(end, shooterRotation)	Pose2d	1
canPass() line 168: new Pose2d[] { ..., ... }	Pose2d[] array	1

Total: roughly 7 + N allocations per cycle, where N = (int) Math.round(start.getDistance(end)) — the ray length in meters. For a robot anywhere on the field this is 5–16 meters, so 12–23 heap objects allocated every 20 ms.

---

Critical issue: the sampling loop

// findIntersectionWithHub, lines 175–184
for (int i = 0; i <= numberOfPoints; i++) {
    ...
    Translation2d point = new Translation2d(x, y);   // ← heap allocation every iteration
    Logger.recordOutput("Raycast/Point", point);      // ← Logger call every iteration too
    if (blueAllianceHub.contains(point) || ...) {
        return point;
    }
}

numberOfPoints is the distance from robot to field wall in meters (computed via Math.round(start.getDistance(end))). At the far end of the field this is ~16 iterations, each allocating a Translation2d and firing a Logger.recordOutput. That's up to 16 allocations + 16 log calls per cycle just from this loop, all of which become garbage immediately. On a 20 ms robot loop this is non-trivial GC pressure.

Fix: Replace the sampling loop with an analytical line-segment vs. AABB intersection. This needs only scalar math — no object allocations in the hot path. The Rectangle2d class in WPILib doesn't expose this directly, but you can compute it yourself with the slab method using the hub's min/max X and Y corners. Zero allocations, no loop, O(1). Fix this →

---

Secondary issue: Logger.recordOutput inside the loop

Logger.recordOutput("Raycast/Point", point) on line 180 fires once per loop iteration. Even if you keep the sampling approach, logging every intermediate point every cycle is extremely chatty and generates additional internal allocations inside AdvantageKit's logging pipeline. This should be moved outside the loop (log only the intersection result if found) or removed entirely.

---

Minor: Pose2d[] array allocated every cycle for visualization

// canPass() line 168
Pose2d[] raycast = new Pose2d[] {robotPose, new Pose2d(end, shooterRotation)};
Logger.recordOutput("Raycast/Line", raycast);

A new Pose2d and a new array are heap-allocated every cycle just to log the ray. Consider caching the array as a field and only updating its contents, or logging the two endpoints separately. This is lower priority than the loop issue.

---

Also: test call in periodic() (pre-existing review note)

// SuperStructure.java line 313
PositionUtils.canPass(robotPose, robotPose.getRotation().plus(Rotation2d.k180deg));

This runs the entire raycasting pipeline every cycle, including all the allocations above, and discards the result. The comment marks it for deletion. Until removed, all the allocation issues above are hitting every cycle unconditionally.

---

Bottom line: The dominant performance problem is the new Translation2d allocation inside findIntersectionWithHub's loop. Replacing it with an analytical intersection eliminates the loop entirely and drops per-cycle allocations from ~20 to ~5.

[Okim29]

@claude is there a way i can solve this raycasting allocation problem without using loops?

[claude]

Claude finished @Okim29's task in 1m 4s —— View job

---

• Read PositionUtils.java
• Analyze current loop and allocations
• Answer question about loop-free raycasting

---

Yes — use the parametric slab method (analytical ray vs. AABB)

The current findIntersectionWithHub samples points along the ray one meter at a time and allocates a Translation2d for each. You can replace the entire loop with pure scalar math that runs in O(1) with zero allocations.

The idea: a ray defined as P(t) = start + t * direction intersects an axis-aligned bounding box (AABB) if and only if the ray's entry t is ≤ its exit t. You compute entry/exit t values for each axis separately using division — no loop needed.

// Pre-cache hub bounds as static final scalars — computed once, never re-allocated
private static final double BLUE_HUB_MIN_X = Hub.nearLeftCorner.getX();
private static final double BLUE_HUB_MAX_X = Hub.farRightCorner.getX();
private static final double BLUE_HUB_MIN_Y = Hub.nearLeftCorner.getY();
private static final double BLUE_HUB_MAX_Y = Hub.farRightCorner.getY();

private static final double RED_HUB_MIN_X = Hub.oppNearLeftCorner.getX();
private static final double RED_HUB_MAX_X = Hub.oppFarRightCorner.getX();
private static final double RED_HUB_MIN_Y = Hub.oppNearLeftCorner.getY();
private static final double RED_HUB_MAX_Y = Hub.oppFarRightCorner.getY();

/** Returns the t-value [0, maxT] where the ray hits the AABB, or -1 if it misses. */
private static double rayHitsBox(
    double startX, double startY,
    double dx, double dy,
    double maxT,
    double minX, double maxX,
    double minY, double maxY) {
  double tXnear = (dx != 0) ? (minX - startX) / dx : Double.NEGATIVE_INFINITY;
  double tXfar  = (dx != 0) ? (maxX - startX) / dx : Double.POSITIVE_INFINITY;
  if (tXnear > tXfar) { double tmp = tXnear; tXnear = tXfar; tXfar = tmp; }

  double tYnear = (dy != 0) ? (minY - startY) / dy : Double.NEGATIVE_INFINITY;
  double tYfar  = (dy != 0) ? (maxY - startY) / dy : Double.POSITIVE_INFINITY;
  if (tYnear > tYfar) { double tmp = tYnear; tYnear = tYfar; tYfar = tmp; }

  double tEnter = Math.max(tXnear, tYnear);
  double tExit  = Math.min(tXfar,  tYfar);

  if (tEnter <= tExit && tExit >= 0 && tEnter <= maxT) {
    return Math.max(tEnter, 0); // clamp to ray start
  }
  return -1;
}

Then canPass becomes:

public static boolean canPass(Pose2d robotPose, Rotation2d shooterRotation) {
  double startX = robotPose.getX();
  double startY = robotPose.getY();
  double dx = shooterRotation.getCos();
  double dy = shooterRotation.getSin();

  // Compute ray length to field boundary (same scalar logic as before)
  double maxT = Double.MAX_VALUE;
  if (dx > 0) maxT = Math.min(maxT, (FieldConstants.fieldLength - startX) / dx);
  else if (dx < 0) maxT = Math.min(maxT, -startX / dx);
  if (dy > 0) maxT = Math.min(maxT, (FieldConstants.fieldWidth - startY) / dy);
  else if (dy < 0) maxT = Math.min(maxT, -startY / dy);

  boolean blocked =
      rayHitsBox(startX, startY, dx, dy, maxT,
                 BLUE_HUB_MIN_X, BLUE_HUB_MAX_X, BLUE_HUB_MIN_Y, BLUE_HUB_MAX_Y) >= 0
   || rayHitsBox(startX, startY, dx, dy, maxT,
                 RED_HUB_MIN_X,  RED_HUB_MAX_X,  RED_HUB_MIN_Y,  RED_HUB_MAX_Y)  >= 0;

  // Optional: log the ray endpoint for visualization (still needs one Pose2d)
  raycast[0] = robotPose;
  double endX = startX + dx * maxT;
  double endY = startY + dy * maxT;
  raycast[1] = new Pose2d(endX, endY, shooterRotation); // 1 alloc, not N
  Logger.recordOutput("Raycast/Line", raycast);

  return !blocked;
}

What this eliminates

Before	After
Loop of N iterations (5–16 per cycle)	No loop — O(1) scalar arithmetic
N × new Translation2d allocations	0 allocations in the hot path
N × Logger.recordOutput calls	0 (log only the final endpoint if needed)
Object reference comparison bug on line 173	Fixed — returns boolean directly

The raycast array field is already cached on line 149, so the only remaining allocation per cycle is the single new Pose2d for the log endpoint. Implement this fix →

[irosenbery]

doesn't look like this is used, can delete

[irosenbery]

Are we going to use these in any other files or code? They can probably go into the constants file

[irosenbery]

Are we going to use this outside the "canPass" function? If not, it is probably better to use a local variable.

[irosenbery]

It looks like this is the only spot we use the raycast array, will it be used by something else in the future?

If not, I don't see the point of creating the array. We can emit the robot pose and raycast details by directly referencing them.

[irosenbery]

the x and y differences here don't need to be recomputed each loop cycle, we can calculate once before the loop and reuse it.

Even simpler, we can calculate the incremental difference in X & Y values per loop and add that difference each loop iteration.

It would look like:

double x = start.getX()
double xIncrement = (end.getX...) * spotOnLine;
double y = start.getY()
doulbe yIncrement ...
for (int i = ...) {
    Translation2d point = ...(x, y);
    ...
    x += xIncrement;
    y += yIncrement;
}

[JRTaylord]

What's this for?