| Austin Schuh | 173a159 | 2018-12-19 16:20:54 +1100 | [diff] [blame] | 1 | #include "frc971/control_loops/fixed_quadrature.h" |
| 2 | |
| 3 | #include <cmath> |
| 4 | |
| 5 | #include "gtest/gtest.h" |
| 6 | |
| Stephan Pleines | f63bde8 | 2024-01-13 15:59:33 -0800 | [diff] [blame] | 7 | namespace frc971::control_loops::testing { |
| Austin Schuh | 173a159 | 2018-12-19 16:20:54 +1100 | [diff] [blame] | 8 | |
| 9 | // Tests that integrating y = cos(x) works. |
| 10 | TEST(GaussianQuadratureTest, Cos) { |
| 11 | double y1 = |
| 12 | GaussianQuadrature5([](double x) { return ::std::cos(x); }, 0.0, 0.5); |
| 13 | |
| 14 | EXPECT_NEAR(y1, ::std::sin(0.5), 1e-15); |
| 15 | } |
| 16 | |
| Austin Schuh | a935a08 | 2023-02-20 21:45:54 -0800 | [diff] [blame] | 17 | // Tests that integrating y = [cos(x), sin(x)] works. |
| 18 | TEST(GaussianQuadratureTest, MatrixCos) { |
| 19 | Eigen::Matrix<double, 2, 1> y1 = MatrixGaussianQuadrature5<2>( |
| 20 | [](double x) { |
| 21 | return Eigen::Matrix<double, 2, 1>(std::cos(x), std::sin(x)); |
| 22 | }, |
| 23 | 0.0, 0.5); |
| 24 | |
| 25 | EXPECT_TRUE(y1.isApprox(Eigen::Matrix<double, 2, 1>( |
| 26 | ::std::sin(0.5), -std::cos(0.5) + std::cos(0)))); |
| 27 | } |
| 28 | |
| Stephan Pleines | f63bde8 | 2024-01-13 15:59:33 -0800 | [diff] [blame] | 29 | } // namespace frc971::control_loops::testing |