Skip to content

Commit

Permalink
Remove wpi::array
Browse files Browse the repository at this point in the history
  • Loading branch information
calcmogul committed Jan 4, 2025
1 parent 98d61e2 commit a44cce3
Show file tree
Hide file tree
Showing 7 changed files with 28 additions and 180 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#pragma once

#include <array>
#include <utility>

#include "trajopt/geometry/Pose2.hpp"
Expand Down Expand Up @@ -36,10 +37,10 @@ class TRAJOPT_DLLEXPORT CubicHermitePoseSplineHolonomic : CubicHermiteSpline {
* @param r0 Initial heading.
* @param r1 Final heading.
*/
CubicHermitePoseSplineHolonomic(wpi::array<double, 2> xInitialControlVector,
wpi::array<double, 2> xFinalControlVector,
wpi::array<double, 2> yInitialControlVector,
wpi::array<double, 2> yFinalControlVector,
CubicHermitePoseSplineHolonomic(std::array<double, 2> xInitialControlVector,
std::array<double, 2> xFinalControlVector,
std::array<double, 2> yInitialControlVector,
std::array<double, 2> yFinalControlVector,
trajopt::Rotation2d r0,
trajopt::Rotation2d r1)
: CubicHermiteSpline(xInitialControlVector, xFinalControlVector,
Expand Down
13 changes: 7 additions & 6 deletions trajoptlib/include/trajopt/spline/CubicHermiteSpline.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,11 @@

#pragma once

#include <array>

#include <Eigen/Core>

#include "trajopt/spline/Spline.hpp"
#include "trajopt/spline/array.hpp"
#include "trajopt/util/SymbolExports.hpp"

namespace frc {
Expand All @@ -29,10 +30,10 @@ class TRAJOPT_DLLEXPORT CubicHermiteSpline : public Spline<3> {
* @param yFinalControlVector The control vector for the final point in
* the y dimension.
*/
CubicHermiteSpline(wpi::array<double, 2> xInitialControlVector,
wpi::array<double, 2> xFinalControlVector,
wpi::array<double, 2> yInitialControlVector,
wpi::array<double, 2> yFinalControlVector);
CubicHermiteSpline(std::array<double, 2> xInitialControlVector,
std::array<double, 2> xFinalControlVector,
std::array<double, 2> yInitialControlVector,
std::array<double, 2> yFinalControlVector);

/**
* Returns the coefficients matrix.
Expand Down Expand Up @@ -110,7 +111,7 @@ class TRAJOPT_DLLEXPORT CubicHermiteSpline : public Spline<3> {
* @return The control vector matrix for a dimension.
*/
static Eigen::Vector4d ControlVectorFromArrays(
wpi::array<double, 2> initialVector, wpi::array<double, 2> finalVector) {
std::array<double, 2> initialVector, std::array<double, 2> finalVector) {
return Eigen::Vector4d{initialVector[0], initialVector[1], finalVector[0],
finalVector[1]};
}
Expand Down
6 changes: 3 additions & 3 deletions trajoptlib/include/trajopt/spline/Spline.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,13 @@

#pragma once

#include <array>
#include <optional>
#include <utility>

#include <Eigen/Core>

#include "trajopt/geometry/Pose2.hpp"
#include "trajopt/spline/array.hpp"

namespace frc {
/**
Expand All @@ -34,10 +34,10 @@ class Spline {
*/
struct ControlVector {
/// The x components of the control vector.
wpi::array<double, (Degree + 1) / 2> x;
std::array<double, (Degree + 1) / 2> x;

/// The y components of the control vector.
wpi::array<double, (Degree + 1) / 2> y;
std::array<double, (Degree + 1) / 2> y;
};

/**
Expand Down
5 changes: 2 additions & 3 deletions trajoptlib/include/trajopt/spline/SplineHelper.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,12 @@

#pragma once

#include <utility>
#include <array>
#include <vector>

#include "trajopt/geometry/Pose2.hpp"
#include "trajopt/geometry/Translation2.hpp"
#include "trajopt/spline/CubicHermiteSpline.hpp"
#include "trajopt/spline/array.hpp"
#include "trajopt/util/SymbolExports.hpp"

namespace frc {
Expand All @@ -27,7 +26,7 @@ class TRAJOPT_DLLEXPORT SplineHelper {
* @param end The ending pose.
* @return 2 cubic control vectors.
*/
static wpi::array<Spline<3>::ControlVector, 2>
static std::array<Spline<3>::ControlVector, 2>
CubicControlVectorsFromWaypoints(
const trajopt::Pose2d& start,
const std::vector<trajopt::Translation2d>& interiorWaypoints,
Expand Down
153 changes: 0 additions & 153 deletions trajoptlib/include/trajopt/spline/array.hpp

This file was deleted.

8 changes: 4 additions & 4 deletions trajoptlib/src/spline/CubicHermiteSpline.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,10 @@
using namespace frc;

CubicHermiteSpline::CubicHermiteSpline(
wpi::array<double, 2> xInitialControlVector,
wpi::array<double, 2> xFinalControlVector,
wpi::array<double, 2> yInitialControlVector,
wpi::array<double, 2> yFinalControlVector)
std::array<double, 2> xInitialControlVector,
std::array<double, 2> xFinalControlVector,
std::array<double, 2> yInitialControlVector,
std::array<double, 2> yFinalControlVector)
: m_initialControlVector{xInitialControlVector, yInitialControlVector},
m_finalControlVector{xFinalControlVector, yFinalControlVector} {
const auto hermite = MakeHermiteBasis();
Expand Down
14 changes: 7 additions & 7 deletions trajoptlib/src/spline/SplineHelper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,10 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
const Spline<3>::ControlVector& end) {
std::vector<CubicHermiteSpline> splines;

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;
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;

if (waypoints.size() > 1) {
waypoints.emplace(waypoints.begin(),
Expand Down Expand Up @@ -93,8 +93,8 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
const double yDeriv =
(3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;

wpi::array<double, 2> midXControlVector{waypoints[0].X(), xDeriv};
wpi::array<double, 2> midYControlVector{waypoints[0].Y(), yDeriv};
std::array<double, 2> midXControlVector{waypoints[0].X(), xDeriv};
std::array<double, 2> midYControlVector{waypoints[0].Y(), yDeriv};

splines.emplace_back(xInitial, midXControlVector, yInitial,
midYControlVector);
Expand All @@ -109,7 +109,7 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
return splines;
}

wpi::array<Spline<3>::ControlVector, 2>
std::array<Spline<3>::ControlVector, 2>
SplineHelper::CubicControlVectorsFromWaypoints(
const trajopt::Pose2d& start,
const std::vector<trajopt::Translation2d>& interiorWaypoints,
Expand Down

0 comments on commit a44cce3

Please sign in to comment.