Manual control point lengths for generatePath feature

pathplannerlib/src/main/java/com/pathplanner/lib/PathPlanner.java

@@ -271,16 +271,19 @@ public static PathPlannerTrajectory generatePath(
 
       double thirdDistance = p1.position.getDistance(p2.position) / 3.0;
 
+      double p1NextDistance = p1.nextControlLength <= 0 ? thirdDistance : p1.nextControlLength;
+      double p2PrevDistance = p2.prevControlLength <= 0 ? thirdDistance : p2.prevControlLength;
+
       Translation2d p1Next =
           p1.position.plus(
               new Translation2d(
-                  p1.heading.getCos() * thirdDistance, p1.heading.getSin() * thirdDistance));
+                  p1.heading.getCos() * p1NextDistance, p1.heading.getSin() * p1NextDistance));
       waypoints.get(i - 1).nextControl = p1Next;
 
       Translation2d p2Prev =
           p2.position.minus(
               new Translation2d(
-                  p2.heading.getCos() * thirdDistance, p2.heading.getSin() * thirdDistance));
+                  p2.heading.getCos() * p2PrevDistance, p2.heading.getSin() * p2PrevDistance));
       waypoints.add(
           new Waypoint(
               p2.position,

pathplannerlib/src/main/java/com/pathplanner/lib/PathPoint.java

@@ -11,6 +11,9 @@ public class PathPoint {
   protected final Rotation2d holonomicRotation;
   protected final double velocityOverride;
 
+  protected double prevControlLength = -1;
+  protected double nextControlLength = -1;
+
   public PathPoint(
       Translation2d position,
       Rotation2d heading,
@@ -34,6 +37,35 @@ public PathPoint(Translation2d position, Rotation2d heading) {
     this(position, heading, Rotation2d.fromDegrees(0));
   }
 
+  public PathPoint withPrevControlLength(double lengthMeters) {
+    if (lengthMeters <= 0) {
+      throw new IllegalArgumentException("Control point lengths must be > 0");
+    }
+
+    prevControlLength = lengthMeters;
+    return this;
+  }
+
+  public PathPoint withNextControlLength(double lengthMeters) {
+    if (lengthMeters <= 0) {
+      throw new IllegalArgumentException("Control point lengths must be > 0");
+    }
+
+    nextControlLength = lengthMeters;
+    return this;
+  }
+
+  public PathPoint withControlLengths(
+      double prevControlLengthMeters, double nextControlLengthMeters) {
+    if (prevControlLengthMeters <= 0 || nextControlLengthMeters <= 0) {
+      throw new IllegalArgumentException("Control point lengths must be > 0");
+    }
+
+    prevControlLength = prevControlLengthMeters;
+    nextControlLength = nextControlLengthMeters;
+    return this;
+  }
+
   public static PathPoint fromCurrentHolonomicState(
       Pose2d currentPose, ChassisSpeeds currentSpeeds) {
     double linearVel =

pathplannerlib/src/main/native/cpp/pathplanner/lib/PathPlanner.cpp

@@ -146,14 +146,21 @@ PathPlannerTrajectory PathPlanner::generatePath(
 		units::meter_t thirdDistance = p1.m_position.Distance(p2.m_position)
 				/ 3.0;
 
+		units::meter_t p1NextDistance =
+				p1.m_nextControlLength <= 0_m ?
+						thirdDistance : p1.m_nextControlLength;
+		units::meter_t p2PrevDistance =
+				p2.m_prevControlLength <= 0_m ?
+						thirdDistance : p2.m_prevControlLength;
+
 		frc::Translation2d p1Next = p1.m_position
-				+ frc::Translation2d(p1.m_heading.Cos() * thirdDistance,
-						p1.m_heading.Sin() * thirdDistance);
+				+ frc::Translation2d(p1.m_heading.Cos() * p1NextDistance,
+						p1.m_heading.Sin() * p1NextDistance);
 		waypoints[i - 1].nextControl = p1Next;
 
 		frc::Translation2d p2Prev = p2.m_position
-				- frc::Translation2d(p2.m_heading.Cos() * thirdDistance,
-						p2.m_heading.Sin() * thirdDistance);
+				- frc::Translation2d(p2.m_heading.Cos() * p2PrevDistance,
+						p2.m_heading.Sin() * p2PrevDistance);
 		waypoints.emplace_back(p2.m_position, p2Prev, frc::Translation2d(),
 				p2.m_velocityOverride, p2.m_holonomicRotation, false, false,
 				PathPlannerTrajectory::StopEvent());

pathplannerlib/src/main/native/include/pathplanner/lib/PathPoint.h

@@ -5,6 +5,8 @@
 #include <frc/geometry/Pose2d.h>
 #include <frc/kinematics/ChassisSpeeds.h>
 #include <units/velocity.h>
+#include <units/length.h>
+#include <frc/Errors.h>
 
 namespace pathplanner {
 class PathPoint {
@@ -14,6 +16,9 @@ class PathPoint {
 	frc::Rotation2d m_holonomicRotation;
 	units::meters_per_second_t m_velocityOverride;
 
+	units::meter_t m_prevControlLength = -1_m;
+	units::meter_t m_nextControlLength = -1_m;
+
 	constexpr PathPoint(frc::Translation2d const position,
 			frc::Rotation2d const heading, frc::Rotation2d holonomicRotation,
 			units::meters_per_second_t velocityOverride) : m_position(position), m_heading(
@@ -37,6 +42,38 @@ class PathPoint {
 			frc::Rotation2d(), -1_mps) {
 	}
 
+	constexpr PathPoint withPrevControlLength(units::meter_t const length) {
+		if (length <= 0_m) {
+			throw FRC_MakeError(frc::err::InvalidParameter,
+					"Control point lengths must be > 0");
+		}
+
+		m_prevControlLength = length;
+		return *this;
+	}
+
+	constexpr PathPoint withNextControlLength(units::meter_t const length) {
+		if (length <= 0_m) {
+			throw FRC_MakeError(frc::err::InvalidParameter,
+					"Control point lengths must be > 0");
+		}
+
+		m_nextControlLength = length;
+		return *this;
+	}
+
+	constexpr PathPoint withControlLengths(units::meter_t const prevLength,
+			units::meter_t const nextLength) {
+		if (prevLength <= 0_m || nextLength <= 0_m) {
+			throw FRC_MakeError(frc::err::InvalidParameter,
+					"Control point lengths must be > 0");
+		}
+
+		m_prevControlLength = prevLength;
+		m_nextControlLength = nextLength;
+		return *this;
+	}
+
 	static PathPoint fromCurrentHolonomicState(frc::Pose2d const currentPose,
 			frc::ChassisSpeeds const currentSpeeds) {
 		units::meters_per_second_t const linearVel = units::math::sqrt(