[wpimath, wpiutil] Add wpi::array for compile time size checking (#3087)

The wpimath APIs use std::array, which doesn't do size checking. Passing
an array with the wrong size can result in uninitialized elements
instead of a compilation error.

This is a breaking change but is worthwhile to avoid hard-to-debug errors.

wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp

@@ -8,7 +8,7 @@ namespace frc {
 
 LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
     const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
-    const std::array<double, 1>& Qelems, const std::array<double, 1>& Relems,
+    const wpi::array<double, 1>& Qelems, const wpi::array<double, 1>& Relems,
     units::second_t dt)
     : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
                                MakeCostMatrix(Relems), dt) {}
@@ -21,7 +21,7 @@ LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
 
 LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
     const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
-    const std::array<double, 2>& Qelems, const std::array<double, 1>& Relems,
+    const wpi::array<double, 2>& Qelems, const wpi::array<double, 1>& Relems,
     units::second_t dt)
     : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
                                MakeCostMatrix(Relems), dt) {}

wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp

@@ -13,9 +13,9 @@ using namespace frc;
 
 DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
     const Rotation2d& gyroAngle, const Pose2d& initialPose,
-    const std::array<double, 5>& stateStdDevs,
-    const std::array<double, 3>& localMeasurementStdDevs,
-    const std::array<double, 3>& visionMeasurmentStdDevs,
+    const wpi::array<double, 5>& stateStdDevs,
+    const wpi::array<double, 3>& localMeasurementStdDevs,
+    const wpi::array<double, 3>& visionMeasurmentStdDevs,
     units::second_t nominalDt)
     : m_observer(
           &DifferentialDrivePoseEstimator::F,
@@ -46,7 +46,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
 }
 
 void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
-    const std::array<double, 3>& visionMeasurmentStdDevs) {
+    const wpi::array<double, 3>& visionMeasurmentStdDevs) {
   // Create R (covariances) for vision measurements.
   Eigen::Matrix<double, 3, 3> visionContR =
       frc::MakeCovMatrix(visionMeasurmentStdDevs);
@@ -126,9 +126,9 @@ Eigen::Matrix<double, 5, 1> DifferentialDrivePoseEstimator::F(
 }
 
 template <int Dim>
-std::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
+wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
     const Eigen::Matrix<double, Dim, 1>& stdDevs) {
-  std::array<double, Dim> array;
+  wpi::array<double, Dim> array;
   for (size_t i = 0; i < Dim; ++i) {
     array[i] = stdDevs(i);
   }

wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp

@@ -7,14 +7,14 @@
 namespace frc {
 
 KalmanFilter<1, 1, 1>::KalmanFilter(
-    LinearSystem<1, 1, 1>& plant, const std::array<double, 1>& stateStdDevs,
-    const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+    LinearSystem<1, 1, 1>& plant, const wpi::array<double, 1>& stateStdDevs,
+    const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
     : detail::KalmanFilterImpl<1, 1, 1>{plant, stateStdDevs, measurementStdDevs,
                                         dt} {}
 
 KalmanFilter<2, 1, 1>::KalmanFilter(
-    LinearSystem<2, 1, 1>& plant, const std::array<double, 2>& stateStdDevs,
-    const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+    LinearSystem<2, 1, 1>& plant, const wpi::array<double, 2>& stateStdDevs,
+    const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
     : detail::KalmanFilterImpl<2, 1, 1>{plant, stateStdDevs, measurementStdDevs,
                                         dt} {}
 

wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp

@@ -14,9 +14,9 @@ using namespace frc;
 frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
     const Rotation2d& gyroAngle, const Pose2d& initialPose,
     MecanumDriveKinematics kinematics,
-    const std::array<double, 3>& stateStdDevs,
-    const std::array<double, 1>& localMeasurementStdDevs,
-    const std::array<double, 3>& visionMeasurementStdDevs,
+    const wpi::array<double, 3>& stateStdDevs,
+    const wpi::array<double, 1>& localMeasurementStdDevs,
+    const wpi::array<double, 3>& visionMeasurementStdDevs,
     units::second_t nominalDt)
     : m_observer([](const Eigen::Matrix<double, 3, 1>& x,
                     const Eigen::Matrix<double, 3, 1>& u) { return u; },
@@ -51,7 +51,7 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
 }
 
 void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
-    const std::array<double, 3>& visionMeasurmentStdDevs) {
+    const wpi::array<double, 3>& visionMeasurmentStdDevs) {
   // Create R (covariances) for vision measurements.
   Eigen::Matrix<double, 3, 3> visionContR =
       frc::MakeCovMatrix(visionMeasurmentStdDevs);

wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp

@@ -7,10 +7,10 @@
 using namespace frc;
 
 CubicHermiteSpline::CubicHermiteSpline(
-    std::array<double, 2> xInitialControlVector,
-    std::array<double, 2> xFinalControlVector,
-    std::array<double, 2> yInitialControlVector,
-    std::array<double, 2> yFinalControlVector) {
+    wpi::array<double, 2> xInitialControlVector,
+    wpi::array<double, 2> xFinalControlVector,
+    wpi::array<double, 2> yInitialControlVector,
+    wpi::array<double, 2> yFinalControlVector) {
   const auto hermite = MakeHermiteBasis();
   const auto x =
       ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);

wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp

@@ -7,10 +7,10 @@
 using namespace frc;
 
 QuinticHermiteSpline::QuinticHermiteSpline(
-    std::array<double, 3> xInitialControlVector,
-    std::array<double, 3> xFinalControlVector,
-    std::array<double, 3> yInitialControlVector,
-    std::array<double, 3> yFinalControlVector) {
+    wpi::array<double, 3> xInitialControlVector,
+    wpi::array<double, 3> xFinalControlVector,
+    wpi::array<double, 3> yInitialControlVector,
+    wpi::array<double, 3> yFinalControlVector) {
   const auto hermite = MakeHermiteBasis();
   const auto x =
       ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);

wpimath/src/main/native/cpp/spline/SplineHelper.cpp

@@ -13,10 +13,10 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
     const Spline<3>::ControlVector& end) {
   std::vector<CubicHermiteSpline> splines;
 
-  std::array<double, 2> xInitial = start.x;
-  std::array<double, 2> yInitial = start.y;
-  std::array<double, 2> xFinal = end.x;
-  std::array<double, 2> yFinal = end.y;
+  wpi::array<double, 2> xInitial = start.x;
+  wpi::array<double, 2> yInitial = start.y;
+  wpi::array<double, 2> xFinal = end.x;
+  wpi::array<double, 2> yFinal = end.y;
 
   if (waypoints.size() > 1) {
     waypoints.emplace(waypoints.begin(),
@@ -102,9 +102,9 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
     const double yDeriv =
         (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
 
-    std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
+    wpi::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
                                             xDeriv};
-    std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
+    wpi::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
                                             yDeriv};
 
     splines.emplace_back(xInitial, midXControlVector, yInitial,
@@ -134,7 +134,7 @@ SplineHelper::QuinticSplinesFromControlVectors(
   return splines;
 }
 
-std::array<Spline<3>::ControlVector, 2>
+wpi::array<Spline<3>::ControlVector, 2>
 SplineHelper::CubicControlVectorsFromWaypoints(
     const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
     const Pose2d& end) {