diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..68e3060 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +packages/ +Debug/ +*/Debug/ +.vs/ \ No newline at end of file diff --git a/FalconLibraryCPP.sln b/FalconLibraryCPP.sln new file mode 100644 index 0000000..409d207 --- /dev/null +++ b/FalconLibraryCPP.sln @@ -0,0 +1,41 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.29009.5 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "FalconLibraryCPP", "FalconLibraryCPP\FalconLibraryCPP.vcxproj", "{9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Tests", "Tests\Tests.vcxproj", "{52260548-392E-4370-8760-D749F5BEBEE6}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Debug|x64.ActiveCfg = Debug|x64 + {9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Debug|x64.Build.0 = Debug|x64 + {9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Debug|x86.ActiveCfg = Debug|Win32 + {9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Debug|x86.Build.0 = Debug|Win32 + {9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Release|x64.ActiveCfg = Release|x64 + {9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Release|x64.Build.0 = Release|x64 + {9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Release|x86.ActiveCfg = Release|Win32 + {9F48B50F-67E0-4F56-99B6-586EA5FFFEBA}.Release|x86.Build.0 = Release|Win32 + {52260548-392E-4370-8760-D749F5BEBEE6}.Debug|x64.ActiveCfg = Debug|x64 + {52260548-392E-4370-8760-D749F5BEBEE6}.Debug|x64.Build.0 = Debug|x64 + {52260548-392E-4370-8760-D749F5BEBEE6}.Debug|x86.ActiveCfg = Debug|Win32 + {52260548-392E-4370-8760-D749F5BEBEE6}.Debug|x86.Build.0 = Debug|Win32 + {52260548-392E-4370-8760-D749F5BEBEE6}.Release|x64.ActiveCfg = Release|x64 + {52260548-392E-4370-8760-D749F5BEBEE6}.Release|x64.Build.0 = Release|x64 + {52260548-392E-4370-8760-D749F5BEBEE6}.Release|x86.ActiveCfg = Release|Win32 + {52260548-392E-4370-8760-D749F5BEBEE6}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {E5EA9E50-DE77-48F2-AF5D-555E9A23AA53} + EndGlobalSection +EndGlobal diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj b/FalconLibraryCPP/FalconLibraryCPP.vcxproj new file mode 100644 index 0000000..c0ad85c --- /dev/null +++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj @@ -0,0 +1,139 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + {9F48B50F-67E0-4F56-99B6-586EA5FFFEBA} + FalconLibraryCPP + 10.0 + + + + Application + true + v142 + MultiByte + + + Application + false + v142 + true + MultiByte + + + Application + true + v142 + MultiByte + + + Application + false + v142 + true + MultiByte + + + + + + + + + + + + + + + + + + + + + + + Level3 + Disabled + true + true + + + Console + + + + + Level3 + Disabled + true + true + + + Console + + + + + Level3 + MaxSpeed + true + true + true + true + + + Console + true + true + + + + + Level3 + MaxSpeed + true + true + true + true + + + Console + true + true + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters new file mode 100644 index 0000000..4f0ba1f --- /dev/null +++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters @@ -0,0 +1,46 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;ipp;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + \ No newline at end of file diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.user b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.user new file mode 100644 index 0000000..966b4ff --- /dev/null +++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.user @@ -0,0 +1,6 @@ + + + + true + + \ No newline at end of file diff --git a/FalconLibraryCPP/src/FalconLibrary.h b/FalconLibraryCPP/src/FalconLibrary.h new file mode 100644 index 0000000..5c3cdef --- /dev/null +++ b/FalconLibraryCPP/src/FalconLibrary.h @@ -0,0 +1,12 @@ +#pragma once + +#include "Interpolatable.h" +#include "Pose2d.h" +#include "Pose2dWithCurvature.h" +#include "Rotation2d.h" +#include "Translation2d.h" +#include "Twist2d.h" +#include "Utilities.h" +#include "VaryInterpolatable.h" + +namespace frc5190 {} \ No newline at end of file diff --git a/FalconLibraryCPP/src/Interpolatable.h b/FalconLibraryCPP/src/Interpolatable.h new file mode 100644 index 0000000..5b88e32 --- /dev/null +++ b/FalconLibraryCPP/src/Interpolatable.h @@ -0,0 +1,17 @@ +#pragma once + +#include "Utilities.h" + +namespace frc5190 { +template +class Interpolatable { + public: + virtual ~Interpolatable() = default; + virtual T Interpolate(const T& end_value, double t) = 0; + + static constexpr double Lerp(const double start_value, const double end_value, + const double t) { + return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0); + } +}; +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/Pose2d.h b/FalconLibraryCPP/src/Pose2d.h new file mode 100644 index 0000000..9e86aa0 --- /dev/null +++ b/FalconLibraryCPP/src/Pose2d.h @@ -0,0 +1,92 @@ +#pragma once + +#include "VaryInterpolatable.h" +#include "Translation2d.h" +#include "Rotation2d.h" +#include "Twist2d.h" +#include "Utilities.h" + +namespace frc5190 { +class Pose2d final : public VaryInterpolatable { + public: + // Constructors + Pose2d() : translation_(Translation2d()), rotation_(Rotation2d()) {} + + Pose2d(Translation2d translation, const Rotation2d rotation) + : translation_(std::move(translation)), rotation_(rotation) {} + + Pose2d(const double x, const double y, const Rotation2d rotation) + : translation_(Translation2d(x, y)), rotation_(rotation) {} + + // Overriden Methods + double Distance(const Pose2d& other) override { + return (-*this + other).AsTwist().Norm(); + } + Pose2d Interpolate(const Pose2d& end_value, const double t) override { + if (t <= 0) { + return *this; + } + if (t >= 1) { + return end_value; + } + const auto twist = (-*this + end_value).AsTwist(); + return *this + (twist * t).AsPose(); + } + + // Operator Overloads + Pose2d operator+(const Pose2d& other) const { return TransformBy(other); } + Pose2d operator-(const Pose2d& other) const { return TransformBy(-other); } + Pose2d operator-() const { + const auto inverted_rotation = -rotation_; + return Pose2d{(-translation_) * inverted_rotation, inverted_rotation}; + } + + // Accessors + const Translation2d& Translation() const { return translation_; } + const Rotation2d& Rotation() const { return rotation_; } + + Twist2d AsTwist() const { + const auto dtheta = rotation_.Radians(); + const auto half_dtheta = dtheta / 2.0; + const auto cos_minus_one = rotation_.Cos() - 1.0; + + double half_theta_by_tan_of_half_dtheta; + + if (std::abs(cos_minus_one) < kEpsilon) { + half_theta_by_tan_of_half_dtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; + } else { + half_theta_by_tan_of_half_dtheta = + -(half_dtheta * rotation_.Sin()) / cos_minus_one; + } + + const auto translation_part = + translation_ * + Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false}; + + return Twist2d{translation_part.X(), translation_part.Y(), + rotation_.Radians()}; + } + + Pose2d Mirror() const { + return Pose2d{Translation2d{translation_.X(), 27.0 - translation_.Y()}, + -rotation_}; + } + + Pose2d TransformBy(const Pose2d& other) const { + return Pose2d{translation_ + (other.translation_ * rotation_), + rotation_ + other.rotation_}; + } + + bool IsCollinear(const Pose2d& other) const { + if (!rotation_.IsParallel(other.rotation_)) { + return false; + } + const auto twist = (-(*this) + other).AsTwist(); + return EpsilonEquals(twist.Dy(), 0.0) && EpsilonEquals(twist.Dtheta(), 0.0); + } + + private: + Translation2d translation_; + Rotation2d rotation_; +}; +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/Pose2dWithCurvature.h b/FalconLibraryCPP/src/Pose2dWithCurvature.h new file mode 100644 index 0000000..153cb43 --- /dev/null +++ b/FalconLibraryCPP/src/Pose2dWithCurvature.h @@ -0,0 +1,46 @@ +#pragma once + +#include +#include "VaryInterpolatable.h" +#include "Pose2d.h" +#include "Interpolatable.h" + +namespace frc5190 { +class Pose2dWithCurvature final : public VaryInterpolatable { + public: + // Constructors + Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds) + : pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {} + + // Overriden Methods + double Distance(const Pose2dWithCurvature& other) override { + return pose_.Distance(other.pose_); + } + + Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value, + double t) override { + return Pose2dWithCurvature{pose_.Interpolate(end_value.pose_, t), + Lerp(curvature_, end_value.curvature_, t), + Lerp(dkds_, end_value.dkds_, t)}; + } + + // Operator Overloads + Pose2dWithCurvature operator+(const Pose2d& other) const { + return Pose2dWithCurvature{pose_ + other, curvature_, dkds_}; + } + + // Accessors + const Pose2d& Pose() const { return pose_; } + double Curvature() const { return curvature_; } + double Dkds() const { return dkds_; } + + Pose2dWithCurvature Mirror() const { + return Pose2dWithCurvature{pose_.Mirror(), -curvature_, -dkds_}; + } + + private: + Pose2d pose_; + double curvature_; + double dkds_; +}; +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/Rotation2d.h b/FalconLibraryCPP/src/Rotation2d.h new file mode 100644 index 0000000..4b99c4c --- /dev/null +++ b/FalconLibraryCPP/src/Rotation2d.h @@ -0,0 +1,61 @@ +#pragma once + +#include + +#include "Utilities.h" + +namespace frc5190 { +class Rotation2d final { + public: + // Constructors + Rotation2d() : value_(0.0), cos_(1.0), sin_(0.0) {} + explicit Rotation2d(const double value) + : value_(value), cos_(std::cos(value)), sin_(std::sin(value)) {} + + Rotation2d(const double x, const double y, const bool normalize) { + if (normalize) { + const auto magnitude = std::hypot(x, y); + if (magnitude > kEpsilon) { + sin_ = y / magnitude; + cos_ = x / magnitude; + } else { + sin_ = 0.0; + cos_ = 1.0; + } + } else { + cos_ = x; + sin_ = y; + } + value_ = std::atan2(sin_, cos_); + } + + static Rotation2d FromDegrees(const double degrees) { + return Rotation2d(Deg2Rad(degrees)); + } + + // Operator Overloads + Rotation2d operator-(const Rotation2d& other) const { return *this + -other; } + Rotation2d operator-() const { return Rotation2d(-value_); } + + Rotation2d operator+(const Rotation2d& other) const { + return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), + Cos() * other.Sin() + Sin() * other.Cos(), true}; + } + + // Accessors + double Radians() const { return value_; } + double Degrees() const { return Rad2Deg(value_); } + double Cos() const { return cos_; } + double Sin() const { return sin_; } + double Tan() const { return sin_ / cos_; } + + bool IsParallel(const Rotation2d& other) const { + return EpsilonEquals((*this - other).Radians(), 0.0); + } + + private: + double value_; + double cos_; + double sin_; +}; +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/Translation2d.h b/FalconLibraryCPP/src/Translation2d.h new file mode 100644 index 0000000..0766053 --- /dev/null +++ b/FalconLibraryCPP/src/Translation2d.h @@ -0,0 +1,69 @@ +#pragma once + +#include + +#include "Interpolatable.h" +#include "VaryInterpolatable.h" +#include "Rotation2d.h" + +namespace frc5190 { + +class Translation2d final : public VaryInterpolatable { + public: + // Constructors + Translation2d() : x_(0.0), y_(0.0) {} + Translation2d(const double x, const double y) : x_(x), y_(y) {} + Translation2d(const double distance, const Rotation2d& rotation) + : x_(distance * rotation.Cos()), y_(distance * rotation.Sin()) {} + + // Overriden Methods + double Distance(const Translation2d& other) override { + return std::hypot(other.X() - X(), other.Y() - Y()); + } + + Translation2d Interpolate(const Translation2d& end_value, + const double t) override { + if (t <= 0) { + return *this; + } + if (t >= 1) { + return end_value; + } + return Translation2d{Lerp(X(), end_value.X(), t), + Lerp(Y(), end_value.Y(), t)}; + } + + // Operator Overloads + Translation2d operator+(const Translation2d& other) const { + return Translation2d{X() + other.X(), Y() + other.Y()}; + } + + Translation2d operator-(const Translation2d& other) const { + return Translation2d{X() - other.X(), Y() - other.Y()}; + } + + Translation2d operator*(const double scalar) const { + return Translation2d{X() * scalar, Y() * scalar}; + } + + Translation2d operator*(const Rotation2d& rotation) const { + return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(), + x_ * rotation.Sin() + y_ * rotation.Cos()}; + } + + Translation2d + operator/(const double scalar) const { + return Translation2d{X() / scalar, Y() / scalar}; + } + + Translation2d operator-() const { return Translation2d{-X(), -Y()}; } + + // Accessors + double X() const { return x_; } + double Y() const { return y_; } + + private: + double x_; + double y_; +}; +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/Twist2d.h b/FalconLibraryCPP/src/Twist2d.h new file mode 100644 index 0000000..9007836 --- /dev/null +++ b/FalconLibraryCPP/src/Twist2d.h @@ -0,0 +1,53 @@ +#pragma once + +#include + +#include "Pose2d.h" +#include "Utilities.h" + +namespace frc5190 { +class Twist2d { + public: + // Constructors + Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {} + Twist2d(const double dx, const double dy, const double dtheta) + : dx_(dx), dy_(dy), dtheta_(dtheta) {} + + // Operator Overloads + Twist2d operator*(const double scalar) const { + return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar}; + } + + // Accessors + double Dx() const { return dx_; } + double Dy() const { return dy_; } + double Dtheta() const { return dtheta_; } + + double Norm() const { + if (dy_ == 0.0) return std::abs(dx_); + return std::hypot(dx_, dy_); + } + + Pose2d AsPose() const { + const auto sin_theta = std::sin(dtheta_); + const auto cos_theta = std::cos(dtheta_); + + double s, c; + if (std::abs(dtheta_) < kEpsilon) { + s = 1.0 - 1.0 / 6.0 * dtheta_ * dtheta_; + c = 0.5 * dtheta_; + } else { + s = sin_theta / dtheta_; + c = (1 - cos_theta) / dtheta_; + } + + return Pose2d{Translation2d{dx_ * s - dy_ * c, dx_ * c + dy_ * s}, + Rotation2d{cos_theta, sin_theta, false}}; + } + + private: + double dx_; + double dy_; + double dtheta_; +}; +} // namespace frc5190 diff --git a/FalconLibraryCPP/src/Utilities.h b/FalconLibraryCPP/src/Utilities.h new file mode 100644 index 0000000..eda2d5a --- /dev/null +++ b/FalconLibraryCPP/src/Utilities.h @@ -0,0 +1,30 @@ +#pragma once + +#include + +namespace frc5190 { + +constexpr double kEpsilon = 1E-9; +constexpr double kPi = 3.14159265358979323846; + +template +T Clamp(const T& n, const T& lower, const T& upper) { + return std::max(lower, std::min(n, upper)); +} + +template +constexpr T Rad2Deg(const T& rad) { + return rad * 180.0 / kPi; +} + +template +constexpr T Deg2Rad(const T& deg) { + return deg * kPi / 180.0; +} + +template +bool EpsilonEquals(const T& a, const T& b) { + return std::abs(a - b) < kEpsilon; +} + +} // namespace frc5190 \ No newline at end of file diff --git a/FalconLibraryCPP/src/VaryInterpolatable.h b/FalconLibraryCPP/src/VaryInterpolatable.h new file mode 100644 index 0000000..9ceb2d3 --- /dev/null +++ b/FalconLibraryCPP/src/VaryInterpolatable.h @@ -0,0 +1,11 @@ +#pragma once + +#include "Interpolatable.h" + +namespace frc5190 { +template +class VaryInterpolatable : public Interpolatable { + public: + virtual double Distance(const T& other) = 0; +}; +} // namespace frc5190 diff --git a/Tests/Tests.vcxproj b/Tests/Tests.vcxproj new file mode 100644 index 0000000..acd09c0 --- /dev/null +++ b/Tests/Tests.vcxproj @@ -0,0 +1,128 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + {52260548-392e-4370-8760-d749f5bebee6} + Win32Proj + 10.0.17763.0 + Application + v142 + Unicode + + + + + + + + + + + + + + Create + Create + Create + Create + + + + + {9f48b50f-67e0-4f56-99b6-586ea5fffeba} + + + + + + + + + + + + + Use + pch.h + Disabled + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + EnableFastChecks + MultiThreadedDebugDLL + Level3 + + + true + Console + + + + + Use + pch.h + Disabled + X64;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + EnableFastChecks + MultiThreadedDebugDLL + Level3 + + + true + Console + + + + + Use + pch.h + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + MultiThreadedDLL + Level3 + ProgramDatabase + + + true + Console + true + true + + + + + Use + pch.h + X64;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + MultiThreadedDLL + Level3 + ProgramDatabase + + + true + Console + true + true + + + + + This project references NuGet package(s) that are missing on this computer. Use NuGet Package Restore to download them. For more information, see http://go.microsoft.com/fwlink/?LinkID=322105. The missing file is {0}. + + + + \ No newline at end of file diff --git a/Tests/Tests.vcxproj.user b/Tests/Tests.vcxproj.user new file mode 100644 index 0000000..88a5509 --- /dev/null +++ b/Tests/Tests.vcxproj.user @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Tests/geometry-tests.cpp b/Tests/geometry-tests.cpp new file mode 100644 index 0000000..65e764c --- /dev/null +++ b/Tests/geometry-tests.cpp @@ -0,0 +1,22 @@ +#include "pch.h" +#include "../FalconLibraryCPP/src/Translation2d.h" +#include "../FalconLibraryCPP/src/Rotation2d.h" + +constexpr double kTestEpsilon = 1E-9; + +TEST(TestRotation2d, TestRotation2d) { + auto rot = frc5190::Rotation2d(); + EXPECT_EQ(1.0, rot.Cos()); + EXPECT_EQ(0.0, rot.Sin()); + EXPECT_EQ(0.0, rot.Tan()); + EXPECT_EQ(0.0, rot.Radians()); + EXPECT_EQ(0.0, rot.Degrees()); + + rot = frc5190::Rotation2d(1, 1, true); + EXPECT_NEAR(std::sqrt(2) / 2, rot.Cos(), kTestEpsilon); + EXPECT_NEAR(std::sqrt(2) / 2, rot.Sin(), kTestEpsilon); + EXPECT_NEAR(1.0, rot.Tan(), kTestEpsilon); + EXPECT_NEAR(45.0, rot.Degrees(), kTestEpsilon); + EXPECT_NEAR(frc5190::kPi / 4, rot.Radians(), kTestEpsilon); + +} \ No newline at end of file diff --git a/Tests/packages.config b/Tests/packages.config new file mode 100644 index 0000000..3c6fe17 --- /dev/null +++ b/Tests/packages.config @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Tests/pch.cpp b/Tests/pch.cpp new file mode 100644 index 0000000..97b544e --- /dev/null +++ b/Tests/pch.cpp @@ -0,0 +1,6 @@ +// +// pch.cpp +// Include the standard header and generate the precompiled header. +// + +#include "pch.h" diff --git a/Tests/pch.h b/Tests/pch.h new file mode 100644 index 0000000..29c81ff --- /dev/null +++ b/Tests/pch.h @@ -0,0 +1,8 @@ +// +// pch.h +// Header for standard system include files. +// + +#pragma once + +#include "gtest/gtest.h" diff --git a/Tests/test.cpp b/Tests/test.cpp new file mode 100644 index 0000000..75492b7 --- /dev/null +++ b/Tests/test.cpp @@ -0,0 +1,21 @@ +#include "pch.h" +#include "../FalconLibraryCPP/src/FalconLibrary.h" + +constexpr double kTestEpsilon = 1E-9; + +TEST(TestRotation2d, TestRotation2d) { + auto rot = frc5190::Rotation2d(); + EXPECT_EQ(1.0, rot.Cos()); + EXPECT_EQ(0.0, rot.Sin()); + EXPECT_EQ(0.0, rot.Tan()); + EXPECT_EQ(0.0, rot.Radians()); + EXPECT_EQ(0.0, rot.Degrees()); + + rot = frc5190::Rotation2d(1, 1, true); + EXPECT_NEAR(std::sqrt(2) / 2, rot.Cos(), kTestEpsilon); + EXPECT_NEAR(std::sqrt(2) / 2, rot.Sin(), kTestEpsilon); + EXPECT_NEAR(1.0, rot.Tan(), kTestEpsilon); + EXPECT_NEAR(45.0, rot.Degrees(), kTestEpsilon); + EXPECT_NEAR(frc5190::kPi / 4, rot.Radians(), kTestEpsilon); + +} \ No newline at end of file