@@ -21,27 +21,37 @@ public class TrajectoryResult {
2121 * visualization. This uses the simulated trajectory from the solver. The
2222 * trajectory naturally terminates when the ball lands at the target (on
2323 * descent). Returns an empty list if no valid solution exists.
24+ *
25+ * <p>When robot velocity is present in the input, the flight path includes
26+ * the inherited chassis velocity so the visualization matches what the
27+ * simulation (FuelSim) actually does with the ball.</p>
2428 */
2529 public List <Pose3d > getFlightPath () {
26- // Only provide a path if the solution is successful and input is available
2730 if (!isSuccess () || input == null ) {
2831 return List .of ();
2932 }
3033
3134 try {
35+ double yaw = input .getShooterYaw () + yawAdjustmentRadians ;
36+ double hSpeed = requiredVelocityMps * Math .cos (pitchAngleRadians );
37+ double launchVx = hSpeed * Math .cos (yaw ) + input .getRobotVx ();
38+ double launchVy = hSpeed * Math .sin (yaw ) + input .getRobotVy ();
39+ double launchVz = requiredVelocityMps * Math .sin (pitchAngleRadians );
40+ double totalLaunchSpeed = Math .sqrt (launchVx * launchVx + launchVy * launchVy + launchVz * launchVz );
41+ double effectivePitch = Math .atan2 (launchVz , Math .sqrt (launchVx * launchVx + launchVy * launchVy ));
42+ double effectiveYaw = Math .atan2 (launchVy , launchVx );
43+
3244 ca .team4308 .absolutelib .math .trajectories .physics .ProjectileMotion projectileMotion
3345 = new ca .team4308 .absolutelib .math .trajectories .physics .ProjectileMotion ();
34- // Pass targetRadius=0 so the simulation runs the full arc to the ground
35- // instead of terminating early at the basket-descent condition.
3646 ca .team4308 .absolutelib .math .trajectories .physics .ProjectileMotion .TrajectoryResult simResult = projectileMotion .simulate (
3747 gamePiece != null ? gamePiece : ca .team4308 .absolutelib .math .trajectories .gamepiece .GamePieces .getCurrent (),
3848 input .getShooterX (), input .getShooterY (), input .getShooterZ (),
39- requiredVelocityMps ,
40- pitchAngleRadians ,
41- input . getShooterYaw () + yawAdjustmentRadians ,
42- 0 , // spinRpm (not tracked in result)
49+ totalLaunchSpeed ,
50+ effectivePitch ,
51+ effectiveYaw ,
52+ 0 ,
4353 input .getTargetX (), input .getTargetY (), input .getTargetZ (),
44- 0 // targetRadius=0 to prevent early termination
54+ 0
4555 );
4656
4757 int validCount = 0 ;
@@ -52,8 +62,6 @@ public List<Pose3d> getFlightPath() {
5262 validCount ++;
5363 }
5464
55- // Find the point of closest 3D approach to the target so the path
56- // ends near the target instead of continuing to the ground.
5765 double tX = input .getTargetX ();
5866 double tY = input .getTargetY ();
5967 double tZ = input .getTargetZ ();
@@ -73,8 +81,6 @@ public List<Pose3d> getFlightPath() {
7381 closestIndex = i ;
7482 }
7583 }
76- // Include a couple points past closest approach so the path visually
77- // reaches the target area, but not the entire descent to the ground.
7884 int endIndex = Math .min (closestIndex + 2 , validCount );
7985
8086 List <Pose3d > path = new ArrayList <>();
0 commit comments