Vector slew rate limiter

[rzblue]

Limit slew rate by interpolating between two vectors

[calcmogul]

Something like this in MathUtil for Translation2d and Translation3d may address the primary use cases:

/**
 * Limits translation velocity.
 *
 * @param current Translation at current timestep.
 * @param next Translation at next timestep.
 * @param dt Timestep duration.
 * @param maxVelocity Maximum translation velocity.
 */
Translation2d SlewRateLimit(const Translation2d& current,
                            const Translation2d& next, units::second_t dt,
                            units::velocity_unit auto maxVelocity) {
  if (maxVelocity < 0_mps) {
    // throw here for precondition violation
  }

  auto diff = next - current;
  auto norm = diff.Norm();

  if (norm < 1e-9_m) {
    return next;
  }

  auto velocity = norm / dt;
  return current + diff * (units::math::min(velocity, maxVelocity) / norm);
}