Add trajectory generation using hermite splines (#1843)

wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp

@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+
+using namespace frc;
+
+CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
+    units::meters_per_second_squared_t maxCentripetalAcceleration)
+    : m_maxCentripetalAcceleration(maxCentripetalAcceleration) {}
+
+units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t velocity) {
+  // ac = v^2 / r
+  // k (curvature) = 1 / r
+
+  // therefore, ac = v^2 * k
+  // ac / k = v^2
+  // v = std::sqrt(ac / k)
+
+  // We have to multiply by 1_rad here to get the units to cancel out nicely.
+  // The units library defines a unit for radians although it is technically
+  // unitless.
+  return units::math::sqrt(m_maxCentripetalAcceleration /
+                           units::math::abs(curvature) * 1_rad);
+}
+
+TrajectoryConstraint::MinMax
+CentripetalAccelerationConstraint::MinMaxAcceleration(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t speed) {
+  // The acceleration of the robot has no impact on the centripetal acceleration
+  // of the robot.
+  return {};
+}