Skip to content

Commit f9ba466

Browse files
Superbro525AltSuperbro525Alt
committed
optimize
1 parent 97c21cc commit f9ba466

6 files changed

Lines changed: 133 additions & 79 deletions

File tree

‎src/main/java/org/curtinfrc/frc2026/util/Repulsor/Shooting/DragShotPlannerCoarseSearch.java‎

Lines changed: 43 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,35 @@ static DragShotPlannerCandidate coarseSearch(
5656
double maxTravelSq = DragShotPlannerConstants.MAX_ROBOT_TRAVEL_METERS_SQ;
5757
double degToRad = DragShotPlannerConstants.DEG_TO_RAD;
5858

59+
int bearingCap = (int) Math.ceil(360.0 / bearingStepDegCoarse);
60+
double[] bearingRad = new double[bearingCap];
61+
double[] bearingCos = new double[bearingCap];
62+
double[] bearingSin = new double[bearingCap];
63+
int bearingCount = 0;
64+
for (double bearingDeg = 0.0; bearingDeg < 360.0 - 1e-9; bearingDeg += bearingStepDegCoarse) {
65+
double rad = bearingDeg * degToRad;
66+
bearingRad[bearingCount] = rad;
67+
bearingCos[bearingCount] = Math.cos(rad);
68+
bearingSin[bearingCount] = Math.sin(rad);
69+
bearingCount++;
70+
}
71+
72+
double angleStartDeg = minAngleDeg;
73+
double angleEndDeg = maxAngleDeg;
74+
double angleStepDeg = fixedAngle ? 1.0 : angleStepCoarse;
75+
int angleCap = (int) Math.ceil((angleEndDeg - angleStartDeg) / angleStepDeg) + 1;
76+
double[] angleRad = new double[angleCap];
77+
double[] angleCos = new double[angleCap];
78+
double[] angleSin = new double[angleCap];
79+
int angleCount = 0;
80+
for (double ang = angleStartDeg; ang <= angleEndDeg + 1e-6; ang += angleStepDeg) {
81+
double rad = ang * degToRad;
82+
angleRad[angleCount] = rad;
83+
angleCos[angleCount] = Math.cos(rad);
84+
angleSin[angleCount] = Math.sin(rad);
85+
angleCount++;
86+
}
87+
5988
DragShotPlannerCandidate bestCoarse = null;
6089

6190
double rx = robotCurrentPosition.getX();
@@ -77,11 +106,11 @@ static DragShotPlannerCandidate coarseSearch(
77106
range <= DragShotPlannerConstants.MAX_RANGE_METERS + 1e-6;
78107
range += radialStepCoarse) {
79108
ranges++;
80-
for (double bearingDeg = 0.0; bearingDeg < 360.0; bearingDeg += bearingStepDegCoarse) {
109+
for (int bi = 0; bi < bearingCount; bi++) {
81110
bearings++;
82-
double bearingRad = bearingDeg * degToRad;
83-
double cosB = Math.cos(bearingRad);
84-
double sinB = Math.sin(bearingRad);
111+
double bearingAngleRad = bearingRad[bi];
112+
double cosB = bearingCos[bi];
113+
double sinB = bearingSin[bi];
85114
double shooterX = targetX - range * cosB;
86115
double shooterY = targetY - range * sinB;
87116
Translation2d shooterPos = new Translation2d(shooterX, shooterY);
@@ -113,32 +142,15 @@ static DragShotPlannerCandidate coarseSearch(
113142
}
114143

115144
double horizontalDistance = range;
116-
double shooterYawRad = bearingRad;
117-
118-
double angleStartDeg;
119-
double angleEndDeg;
120-
double angleStepDeg;
121-
122-
if (fixedAngle) {
123-
angleStartDeg = minAngleDeg;
124-
angleEndDeg = maxAngleDeg;
125-
angleStepDeg = 1.0;
126-
} else {
127-
angleStartDeg = minAngleDeg;
128-
angleEndDeg = maxAngleDeg;
129-
angleStepDeg = angleStepCoarse;
130-
}
131-
132-
for (double angleDeg = angleStartDeg;
133-
angleDeg <= angleEndDeg + 1e-6;
134-
angleDeg += angleStepDeg) {
145+
double shooterYawRad = bearingAngleRad;
135146

136-
double angleRad = angleDeg * degToRad;
137-
double cos = Math.cos(angleRad);
147+
for (int ai = 0; ai < angleCount; ai++) {
148+
double cos = angleCos[ai];
138149
if (cos <= 0.0) {
139150
continue;
140151
}
141-
double sin = Math.sin(angleRad);
152+
double sin = angleSin[ai];
153+
double angleRadVal = angleRad[ai];
142154

143155
for (double speed = minSpeed; speed <= maxSpeed + 1e-6; speed += speedStepCoarse) {
144156
sims++;
@@ -161,7 +173,10 @@ static DragShotPlannerCandidate coarseSearch(
161173
}
162174
simsHit++;
163175

164-
double error = Math.abs(sim.verticalErrorMeters);
176+
double error = sim.verticalErrorMeters;
177+
if (error < 0.0) {
178+
error = -error;
179+
}
165180
if (error > coarseTolerance) {
166181
continue;
167182
}
@@ -172,7 +187,7 @@ static DragShotPlannerCandidate coarseSearch(
172187
shooterPos,
173188
shooterYawRad,
174189
speed,
175-
angleRad,
190+
angleRadVal,
176191
sim.timeAtPlaneSeconds,
177192
error,
178193
robotDistanceSq);

‎src/main/java/org/curtinfrc/frc2026/util/Repulsor/Shooting/DragShotPlannerLibrarySearch.java‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -102,14 +102,18 @@ static Optional<ShotSolution> findBestShotFromLibrary(
102102
}
103103
}
104104

105+
double err = e.verticalErrorMeters();
106+
if (err < 0.0) {
107+
err = -err;
108+
}
105109
DragShotPlannerCandidate next =
106110
new DragShotPlannerCandidate(
107111
shooterPos,
108112
e.shooterYawRad(),
109113
e.launchSpeedMetersPerSecond(),
110114
e.launchAngleRad(),
111115
e.timeToPlaneSeconds(),
112-
Math.abs(e.verticalErrorMeters()),
116+
err,
113117
robotDistanceSq);
114118

115119
if (DragShotPlannerCandidate.isBetterCandidate(best, next, shotStyle)) {

‎src/main/java/org/curtinfrc/frc2026/util/Repulsor/Shooting/DragShotPlannerObstacles.java‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,9 @@ static boolean isShooterPoseValidInternal(
7373
return false;
7474
}
7575

76-
Rotation2d yaw = targetFieldPosition.minus(shooterPos).getAngle();
76+
double dx = targetFieldPosition.getX() - x;
77+
double dy = targetFieldPosition.getY() - y;
78+
Rotation2d yaw = Rotation2d.fromRadians(Math.atan2(dy, dx));
7779
Translation2d[] rect =
7880
FieldPlanner.robotRect(shooterPos, yaw, robotHalfLengthMeters, robotHalfWidthMeters);
7981

‎src/main/java/org/curtinfrc/frc2026/util/Repulsor/Shooting/DragShotPlannerOnlineSearch.java‎

Lines changed: 31 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -88,10 +88,12 @@ static Optional<ShotSolution> findBestShotOnlineRefine(
8888
Translation2d robotPos = robotPose.getTranslation();
8989
double robotX = robotPos.getX();
9090
double robotY = robotPos.getY();
91+
double targetX = targetFieldPosition.getX();
92+
double targetY = targetFieldPosition.getY();
9193

9294
Translation2d seed = state.seed();
9395
if (seed == null) {
94-
seed = projectToValidRing(robotPos, targetFieldPosition);
96+
seed = projectToValidRing(robotX, robotY, targetX, targetY);
9597
state.seed(seed);
9698
}
9799

@@ -142,14 +144,18 @@ static Optional<ShotSolution> findBestShotOnlineRefine(
142144
double dy = robotY - seedSol.shooterPosition().getY();
143145
double distSq = dx * dx + dy * dy;
144146
if (distSq <= maxTravelSq + 1e-6) {
147+
double errSeed = seedSol.verticalErrorMeters();
148+
if (errSeed < 0.0) {
149+
errSeed = -errSeed;
150+
}
145151
best =
146152
new DragShotPlannerCandidate(
147153
seedSol.shooterPosition(),
148154
seedSol.shooterYaw().getRadians(),
149155
seedSol.launchSpeedMetersPerSecond(),
150156
seedSol.launchAngle().getRadians(),
151157
seedSol.timeToPlaneSeconds(),
152-
Math.abs(seedSol.verticalErrorMeters()),
158+
errSeed,
153159
distSq);
154160
}
155161
}
@@ -175,11 +181,14 @@ static Optional<ShotSolution> findBestShotOnlineRefine(
175181

176182
for (double[] o : ONLINE_OFFS) {
177183
inner++;
178-
Translation2d p = new Translation2d(cx + o[0] * step, cy + o[1] * step);
179-
Translation2d clipped = clipToRingAndField(p, targetFieldPosition);
180-
181-
double dxr = robotX - clipped.getX();
182-
double dyr = robotY - clipped.getY();
184+
double px = cx + o[0] * step;
185+
double py = cy + o[1] * step;
186+
Translation2d clipped = clipToRingAndField(px, py, targetX, targetY);
187+
188+
double clippedX = clipped.getX();
189+
double clippedY = clipped.getY();
190+
double dxr = robotX - clippedX;
191+
double dyr = robotY - clippedY;
183192
double distSq = dxr * dxr + dyr * dyr;
184193
if (distSq > maxTravelSq) {
185194
rejectedTravel++;
@@ -236,14 +245,18 @@ static Optional<ShotSolution> findBestShotOnlineRefine(
236245
double dy2 = robotY - sol.shooterPosition().getY();
237246
double distSq2 = dx2 * dx2 + dy2 * dy2;
238247

248+
double errSol = sol.verticalErrorMeters();
249+
if (errSol < 0.0) {
250+
errSol = -errSol;
251+
}
239252
DragShotPlannerCandidate cand =
240253
new DragShotPlannerCandidate(
241254
sol.shooterPosition(),
242255
sol.shooterYaw().getRadians(),
243256
sol.launchSpeedMetersPerSecond(),
244257
sol.launchAngle().getRadians(),
245258
sol.timeToPlaneSeconds(),
246-
Math.abs(sol.verticalErrorMeters()),
259+
errSol,
247260
distSq2);
248261

249262
if (DragShotPlannerCandidate.isBetterCandidate(best, cand, shotStyle)) {
@@ -319,39 +332,37 @@ static Optional<ShotSolution> findBestShotOnlineRefine(
319332
}
320333
}
321334

322-
private static Translation2d projectToValidRing(Translation2d point, Translation2d target) {
335+
private static Translation2d projectToValidRing(
336+
double px, double py, double targetX, double targetY) {
323337
AutoCloseable _p = Profiler.section("DragShotPlanner.projectToValidRing");
324338
try {
325-
double px = point.getX();
326-
double py = point.getY();
327-
double tx = target.getX();
328-
double ty = target.getY();
329-
double dx = px - tx;
330-
double dy = py - ty;
339+
double dx = px - targetX;
340+
double dy = py - targetY;
331341
double r2 = dx * dx + dy * dy;
332342
if (r2 < 1e-12) {
333-
return new Translation2d(tx - 3.0, ty);
343+
return new Translation2d(targetX - 3.0, targetY);
334344
}
335345
double r = Math.sqrt(r2);
336346
double clamped =
337347
Math.max(
338348
DragShotPlannerConstants.MIN_RANGE_METERS,
339349
Math.min(DragShotPlannerConstants.MAX_RANGE_METERS, r));
340350
double scale = clamped / r;
341-
return new Translation2d(tx + dx * scale, ty + dy * scale);
351+
return new Translation2d(targetX + dx * scale, targetY + dy * scale);
342352
} finally {
343353
DragShotPlannerUtil.closeQuietly(_p);
344354
}
345355
}
346356

347-
private static Translation2d clipToRingAndField(Translation2d p, Translation2d target) {
357+
private static Translation2d clipToRingAndField(
358+
double px, double py, double targetX, double targetY) {
348359
AutoCloseable _p0 = Profiler.section("DragShotPlanner.clipToRingAndField");
349360
try {
350-
Translation2d ring = projectToValidRing(p, target);
361+
Translation2d ring = projectToValidRing(px, py, targetX, targetY);
351362
double x = ring.getX();
352363
double y = ring.getY();
353364
if (!Double.isFinite(x) || !Double.isFinite(y)) {
354-
return new Translation2d(target.getX() - 3.0, target.getY());
365+
return new Translation2d(targetX - 3.0, targetY);
355366
}
356367
return ring;
357368
} finally {

‎src/main/java/org/curtinfrc/frc2026/util/Repulsor/Shooting/DragShotPlannerRefineAtPosition.java‎

Lines changed: 21 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -55,10 +55,6 @@ static ShotSolution refineShotAtPosition(
5555
return null;
5656
}
5757

58-
double invD = 1.0 / horizontalDistance;
59-
double dirUx = dxT * invD;
60-
double dirUy = dyT * invD;
61-
6258
Rotation2d shooterYaw = Rotation2d.fromRadians(Math.atan2(dyT, dxT));
6359

6460
double speedWindow = Math.max(1.5, (maxSpeed - minSpeed) / 8.0);
@@ -94,16 +90,26 @@ static ShotSolution refineShotAtPosition(
9490

9591
DragShotPlannerSimulation.SimOut sim = DragShotPlannerSimulation.simOut();
9692

97-
for (double angleDeg = angleStartDeg;
98-
angleDeg <= angleEndDeg + 1e-6;
99-
angleDeg += angleStepFineDeg) {
93+
int angleCap = (int) Math.ceil((angleEndDeg - angleStartDeg) / angleStepFineDeg) + 1;
94+
double[] angleRad = new double[angleCap];
95+
double[] angleCos = new double[angleCap];
96+
double[] angleSin = new double[angleCap];
97+
int angleCount = 0;
98+
for (double ang = angleStartDeg; ang <= angleEndDeg + 1e-6; ang += angleStepFineDeg) {
99+
double rad = ang * degToRad;
100+
angleRad[angleCount] = rad;
101+
angleCos[angleCount] = Math.cos(rad);
102+
angleSin[angleCount] = Math.sin(rad);
103+
angleCount++;
104+
}
100105

101-
double angleRad = angleDeg * degToRad;
102-
double cos = Math.cos(angleRad);
106+
for (int ai = 0; ai < angleCount; ai++) {
107+
double cos = angleCos[ai];
103108
if (cos <= 0.0) {
104109
continue;
105110
}
106-
double sin = Math.sin(angleRad);
111+
double sin = angleSin[ai];
112+
double angleRad = angleRad[ai];
107113

108114
for (double speed = speedMin; speed <= speedMax + 1e-6; speed += speedStepFine) {
109115
sims++;
@@ -126,7 +132,10 @@ static ShotSolution refineShotAtPosition(
126132
}
127133
simsHit++;
128134

129-
double error = Math.abs(sim.verticalErrorMeters);
135+
double error = sim.verticalErrorMeters;
136+
if (error < 0.0) {
137+
error = -error;
138+
}
130139
if (error > acceptableError) {
131140
continue;
132141
}
@@ -136,8 +145,6 @@ static ShotSolution refineShotAtPosition(
136145
|| error < bestError - DragShotPlannerConstants.EPS
137146
|| (Math.abs(error - bestError) <= DragShotPlannerConstants.EPS
138147
&& speed < best.launchSpeedMetersPerSecond() - DragShotPlannerConstants.EPS)) {
139-
Translation2d impactPos =
140-
new Translation2d(sx + dirUx * horizontalDistance, sy + dirUy * horizontalDistance);
141148
bestError = error;
142149
best =
143150
new ShotSolution(
@@ -146,7 +153,7 @@ static ShotSolution refineShotAtPosition(
146153
speed,
147154
Rotation2d.fromRadians(angleRad),
148155
sim.timeAtPlaneSeconds,
149-
impactPos,
156+
targetFieldPosition,
150157
sim.verticalErrorMeters);
151158
}
152159
}

0 commit comments

Comments
 (0)