Add back old ctor and calculate and deprecate them

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java

@@ -44,6 +44,8 @@ public class TrapezoidProfile {
 
   private final Constraints m_constraints;
   private State m_current;
+  private State m_goal; // TODO: Remove
+  private final boolean m_newAPI; // TODO: Remove
 
   private double m_endAccel;
   private double m_endFullSpeed;
@@ -102,6 +104,107 @@ public int hashCode() {
    */
   public TrapezoidProfile(Constraints constraints) {
     m_constraints = constraints;
+    m_newAPI = true;
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal The desired state when the profile is complete.
+   * @param initial The initial state (usually the current state).
+   * @deprecated Pass the desired and current state into calculate instead of constructing a new
+   *     TrapezoidProfile with the desired and current state
+   */
+  @Deprecated(since = "2023", forRemoval = true)
+  public TrapezoidProfile(Constraints constraints, State goal, State initial) {
+    m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
+    m_constraints = constraints;
+    m_current = direct(initial);
+    m_goal = direct(goal);
+    m_newAPI = false;
+    if (m_current.velocity > m_constraints.maxVelocity) {
+      m_current.velocity = m_constraints.maxVelocity;
+    }
+
+    // Deal with a possibly truncated motion profile (with nonzero initial or
+    // final velocity) by calculating the parameters as if the profile began and
+    // ended at zero velocity
+    double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
+    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+    double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+    double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+    // Now we can calculate the parameters as if it was a full trapezoid instead
+    // of a truncated one
+
+    double fullTrapezoidDist =
+        cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
+    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+    double fullSpeedDist =
+        fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+    // Handle the case where the profile never reaches full speed
+    if (fullSpeedDist < 0) {
+      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+      fullSpeedDist = 0;
+    }
+
+    m_endAccel = accelerationTime - cutoffBegin;
+    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal The desired state when the profile is complete.
+   * @deprecated Pass the desired and current state into calculate instead of constructing a new
+   *     TrapezoidProfile with the desired and current state
+   */
+  @Deprecated(since = "2023", forRemoval = true)
+  public TrapezoidProfile(Constraints constraints, State goal) {
+    this(constraints, goal, new State(0, 0));
+  }
+
+  /**
+   * Calculate the correct position and velocity for the profile at a time t where the beginning of
+   * the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   * @return The position and velocity of the profile at time t.
+   * @deprecated Pass the desired and current state into calculate instead of constructing a new
+   *     TrapezoidProfile with the desired and current state
+   */
+  @Deprecated(since = "2023", forRemoval = true)
+  public State calculate(double t) {
+    if (m_newAPI) {
+      throw new RuntimeException("Cannot use new constructor with deprecated calculate()");
+    }
+    State result = new State(m_current.position, m_current.velocity);
+
+    if (t < m_endAccel) {
+      result.velocity += t * m_constraints.maxAcceleration;
+      result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+    } else if (t < m_endFullSpeed) {
+      result.velocity = m_constraints.maxVelocity;
+      result.position +=
+          (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+              + m_constraints.maxVelocity * (t - m_endAccel);
+    } else if (t <= m_endDeccel) {
+      result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+      double timeLeft = m_endDeccel - t;
+      result.position =
+          m_goal.position
+              - (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft;
+    } else {
+      result = m_goal;
+    }
+
+    return direct(result);
   }
 
   /**

wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <wpi/deprecated.h>
+
 #include "units/time.h"
 #include "wpimath/MathShared.h"
 
@@ -79,11 +81,41 @@ class TrapezoidProfile {
    */
   TrapezoidProfile(Constraints constraints);  // NOLINT
 
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal        The desired state when the profile is complete.
+   * @param initial     The initial state (usually the current state).
+   * @deprecated Pass the desired and current state into calculate instead of
+   * constructing a new TrapezoidProfile with the desired and current state
+   */
+  WPI_DEPRECATED(
+      "Pass the desired and current state into calculate instead of "
+      "constructing a new TrapezoidProfile with the desired and current "
+      "state")
+  TrapezoidProfile(Constraints constraints, State goal,
+                   State initial = State{Distance_t{0}, Velocity_t{0}});
+
   TrapezoidProfile(const TrapezoidProfile&) = default;
   TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
   TrapezoidProfile(TrapezoidProfile&&) = default;
   TrapezoidProfile& operator=(TrapezoidProfile&&) = default;
 
+  /**
+   * Calculate the correct position and velocity for the profile at a time t
+   * where the beginning of the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   * @deprecated Pass the desired and current state into calculate instead of
+   * constructing a new TrapezoidProfile with the desired and current state
+   */
+  [[deprecated(
+      "Pass the desired and current state into calculate instead of "
+      "constructing a new TrapezoidProfile with the desired and current "
+      "state")]]
+  State Calculate(units::second_t t) const;
+
   /**
    * Calculate the correct position and velocity for the profile at a time t
    * where the beginning of the profile was at time t = 0.
@@ -141,7 +173,9 @@ class TrapezoidProfile {
   int m_direction;
 
   Constraints m_constraints;
-  State m_initial;
+  State m_current;
+  State m_goal;   // TODO: remove
+  bool m_newAPI;  // TODO: remove
 
   units::second_t m_endAccel;
   units::second_t m_endFullSpeed;

wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc

@@ -12,24 +12,105 @@
 namespace frc {
 template <class Distance>
 TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints)
-    : m_constraints(constraints) {}
+    : m_constraints(constraints), m_newAPI(true) {}
 
+template <class Distance>
+TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
+                                             State goal, State initial)
+    : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
+      m_constraints(constraints),
+      m_current(Direct(initial)),
+      m_goal(Direct(goal)),
+      m_newAPI(false) {
+  if (m_current.velocity > m_constraints.maxVelocity) {
+    m_current.velocity = m_constraints.maxVelocity;
+  }
+
+  // Deal with a possibly truncated motion profile (with nonzero initial or
+  // final velocity) by calculating the parameters as if the profile began and
+  // ended at zero velocity
+  units::second_t cutoffBegin =
+      m_current.velocity / m_constraints.maxAcceleration;
+  Distance_t cutoffDistBegin =
+      cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+  units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+  Distance_t cutoffDistEnd =
+      cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+  // Now we can calculate the parameters as if it was a full trapezoid instead
+  // of a truncated one
+
+  Distance_t fullTrapezoidDist =
+      cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
+  units::second_t accelerationTime =
+      m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+  Distance_t fullSpeedDist =
+      fullTrapezoidDist -
+      accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+  // Handle the case where the profile never reaches full speed
+  if (fullSpeedDist < Distance_t{0}) {
+    accelerationTime =
+        units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+    fullSpeedDist = Distance_t{0};
+  }
+
+  m_endAccel = accelerationTime - cutoffBegin;
+  m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+  m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+}
+
+template <class Distance>
+typename TrapezoidProfile<Distance>::State
+TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
+  if (m_newAPI) {
+    throw std::runtime_error(
+        "Cannot use new constructor with deprecated Calculate()");
+  }
+  State result = m_current;
+
+  if (t < m_endAccel) {
+    result.velocity += t * m_constraints.maxAcceleration;
+    result.position +=
+        (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+  } else if (t < m_endFullSpeed) {
+    result.velocity = m_constraints.maxVelocity;
+    result.position += (m_current.velocity +
+                        m_endAccel * m_constraints.maxAcceleration / 2.0) *
+                           m_endAccel +
+                       m_constraints.maxVelocity * (t - m_endAccel);
+  } else if (t <= m_endDeccel) {
+    result.velocity =
+        m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+    units::second_t timeLeft = m_endDeccel - t;
+    result.position =
+        m_goal.position -
+        (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
+            timeLeft;
+  } else {
+    result = m_goal;
+  }
+
+  return Direct(result);
+}
 template <class Distance>
 typename TrapezoidProfile<Distance>::State
 TrapezoidProfile<Distance>::Calculate(units::second_t t, State goal,
                                       State current) {
   m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1;
-  m_initial = Direct(current);
+  m_current = Direct(current);
   goal = Direct(goal);
-  if (m_initial.velocity > m_constraints.maxVelocity) {
-    m_initial.velocity = m_constraints.maxVelocity;
+  if (m_current.velocity > m_constraints.maxVelocity) {
+    m_current.velocity = m_constraints.maxVelocity;
   }
 
   // Deal with a possibly truncated motion profile (with nonzero initial or
   // final velocity) by calculating the parameters as if the profile began and
   // ended at zero velocity
   units::second_t cutoffBegin =
-      m_initial.velocity / m_constraints.maxAcceleration;
+      m_current.velocity / m_constraints.maxAcceleration;
   Distance_t cutoffDistBegin =
       cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
 
@@ -41,7 +122,7 @@ TrapezoidProfile<Distance>::Calculate(units::second_t t, State goal,
   // of a truncated one
 
   Distance_t fullTrapezoidDist =
-      cutoffDistBegin + (goal.position - m_initial.position) + cutoffDistEnd;
+      cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
   units::second_t accelerationTime =
       m_constraints.maxVelocity / m_constraints.maxAcceleration;
 
@@ -59,15 +140,15 @@ TrapezoidProfile<Distance>::Calculate(units::second_t t, State goal,
   m_endAccel = accelerationTime - cutoffBegin;
   m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
   m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
-  State result = m_initial;
+  State result = m_current;
 
   if (t < m_endAccel) {
     result.velocity += t * m_constraints.maxAcceleration;
     result.position +=
-        (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+        (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
   } else if (t < m_endFullSpeed) {
     result.velocity = m_constraints.maxVelocity;
-    result.position += (m_initial.velocity +
+    result.position += (m_current.velocity +
                         m_endAccel * m_constraints.maxAcceleration / 2.0) *
                            m_endAccel +
                        m_constraints.maxVelocity * (t - m_endAccel);
@@ -89,8 +170,8 @@ TrapezoidProfile<Distance>::Calculate(units::second_t t, State goal,
 template <class Distance>
 units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
     Distance_t target) const {
-  Distance_t position = m_initial.position * m_direction;
-  Velocity_t velocity = m_initial.velocity * m_direction;
+  Distance_t position = m_current.position * m_direction;
+  Velocity_t velocity = m_current.velocity * m_direction;
 
   units::second_t endAccel = m_endAccel * m_direction;
   units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;