diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..92b4049 --- /dev/null +++ b/.clang-format @@ -0,0 +1,107 @@ +--- +Language: Cpp +BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: true +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeCategories: + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseLabels: true +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +ReflowComments: true +SortIncludes: false +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +TabWidth: 8 +UseTab: Never +... diff --git a/FalconLibraryCPP/src/Pose2d.h b/FalconLibraryCPP/src/Pose2d.h index 9e86aa0..381b173 100644 --- a/FalconLibraryCPP/src/Pose2d.h +++ b/FalconLibraryCPP/src/Pose2d.h @@ -3,10 +3,11 @@ #include "VaryInterpolatable.h" #include "Translation2d.h" #include "Rotation2d.h" -#include "Twist2d.h" #include "Utilities.h" +#include "Twist2d.h" namespace frc5190 { +class Twist2d; class Pose2d final : public VaryInterpolatable { public: // Constructors @@ -20,7 +21,7 @@ class Pose2d final : public VaryInterpolatable { // Overriden Methods double Distance(const Pose2d& other) override { - return (-*this + other).AsTwist().Norm(); + return ToTwist(-*this + other).Norm(); } Pose2d Interpolate(const Pose2d& end_value, const double t) override { if (t <= 0) { @@ -29,8 +30,8 @@ class Pose2d final : public VaryInterpolatable { if (t >= 1) { return end_value; } - const auto twist = (-*this + end_value).AsTwist(); - return *this + (twist * t).AsPose(); + const auto twist = ToTwist(-*this + end_value); + return *this + FromTwist(twist * t); } // Operator Overloads @@ -45,28 +46,6 @@ class Pose2d final : public VaryInterpolatable { 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_}; @@ -81,10 +60,52 @@ class Pose2d final : public VaryInterpolatable { if (!rotation_.IsParallel(other.rotation_)) { return false; } - const auto twist = (-(*this) + other).AsTwist(); + const auto twist = ToTwist(-(*this) + other); return EpsilonEquals(twist.Dy(), 0.0) && EpsilonEquals(twist.Dtheta(), 0.0); } + // Static Methods + static Twist2d ToTwist(const Pose2d& pose) { + const auto dtheta = pose.rotation_.Radians(); + const auto half_dtheta = dtheta / 2.0; + const auto cos_minus_one = pose.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 * pose.rotation_.Sin()) / cos_minus_one; + } + + const auto translation_part = + pose.translation_ * + Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false}; + + return Twist2d{translation_part.X(), translation_part.Y(), + pose.rotation_.Radians()}; + } + + static Pose2d FromTwist(const Twist2d& twist) { + const auto dx = twist.Dx(), dy = twist.Dy(), dtheta = twist.Dtheta(); + + 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: Translation2d translation_; Rotation2d rotation_; diff --git a/FalconLibraryCPP/src/Pose2dWithCurvature.h b/FalconLibraryCPP/src/Pose2dWithCurvature.h index 153cb43..3575f14 100644 --- a/FalconLibraryCPP/src/Pose2dWithCurvature.h +++ b/FalconLibraryCPP/src/Pose2dWithCurvature.h @@ -6,7 +6,8 @@ #include "Interpolatable.h" namespace frc5190 { -class Pose2dWithCurvature final : public VaryInterpolatable { +class Pose2dWithCurvature final + : public VaryInterpolatable { public: // Constructors Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds) diff --git a/FalconLibraryCPP/src/Translation2d.h b/FalconLibraryCPP/src/Translation2d.h index 0766053..3dd4028 100644 --- a/FalconLibraryCPP/src/Translation2d.h +++ b/FalconLibraryCPP/src/Translation2d.h @@ -51,8 +51,7 @@ class Translation2d final : public VaryInterpolatable { x_ * rotation.Sin() + y_ * rotation.Cos()}; } - Translation2d - operator/(const double scalar) const { + Translation2d operator/(const double scalar) const { return Translation2d{X() / scalar, Y() / scalar}; } diff --git a/FalconLibraryCPP/src/Twist2d.h b/FalconLibraryCPP/src/Twist2d.h index 9007836..150cd3c 100644 --- a/FalconLibraryCPP/src/Twist2d.h +++ b/FalconLibraryCPP/src/Twist2d.h @@ -2,9 +2,6 @@ #include -#include "Pose2d.h" -#include "Utilities.h" - namespace frc5190 { class Twist2d { public: @@ -28,23 +25,6 @@ class Twist2d { 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_; diff --git a/Tests/geometry-tests.cpp b/Tests/geometry-tests.cpp index 65e764c..64b5288 100644 --- a/Tests/geometry-tests.cpp +++ b/Tests/geometry-tests.cpp @@ -1,22 +1,22 @@ #include "pch.h" -#include "../FalconLibraryCPP/src/Translation2d.h" -#include "../FalconLibraryCPP/src/Rotation2d.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()); + 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); + 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); + auto twist = frc5190::Twist2d(); } \ No newline at end of file diff --git a/Tests/test.cpp b/Tests/test.cpp deleted file mode 100644 index 75492b7..0000000 --- a/Tests/test.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#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