Fix compilation errors

.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
+...

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<Pose2d> {
  public:
   // Constructors
@@ -20,7 +21,7 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
 
   // 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<Pose2d> {
     if (t >= 1) {
       return end_value;
     }
-    const auto twist = (-*this + end_value).AsTwist();
+    const auto twist = ToTwist(-*this + end_value);
-    return *this + (twist * t).AsPose();
+    return *this + FromTwist(twist * t);
   }
 
   // Operator Overloads
@@ -45,28 +46,6 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
   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<Pose2d> {
     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_;

FalconLibraryCPP/src/Pose2dWithCurvature.h

@@ -6,7 +6,8 @@
 #include "Interpolatable.h"
 
 namespace frc5190 {
-class Pose2dWithCurvature final : public VaryInterpolatable<Pose2dWithCurvature> {
+class Pose2dWithCurvature final
+    : public VaryInterpolatable<Pose2dWithCurvature> {
  public:
   // Constructors
   Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds)

FalconLibraryCPP/src/Translation2d.h

@@ -51,8 +51,7 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
                          x_ * rotation.Sin() + y_ * rotation.Cos()};
   }
 
-      Translation2d
+  Translation2d operator/(const double scalar) const {
-      operator/(const double scalar) const {
     return Translation2d{X() / scalar, Y() / scalar};
   }
 

FalconLibraryCPP/src/Twist2d.h

@@ -2,9 +2,6 @@
 
 #include <cmath>
 
-#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_;

Tests/geometry-tests.cpp

@@ -1,22 +1,22 @@
 #include "pch.h"
-#include "../FalconLibraryCPP/src/Translation2d.h"
+#include "../FalconLibraryCPP/src/FalconLibrary.h"
-#include "../FalconLibraryCPP/src/Rotation2d.h"
 
 constexpr double kTestEpsilon = 1E-9;
 
 TEST(TestRotation2d, TestRotation2d) {
-	auto rot = frc5190::Rotation2d();
+  auto rot = frc5190::Rotation2d();
-	EXPECT_EQ(1.0, rot.Cos());
+  EXPECT_EQ(1.0, rot.Cos());
-	EXPECT_EQ(0.0, rot.Sin());
+  EXPECT_EQ(0.0, rot.Sin());
-	EXPECT_EQ(0.0, rot.Tan());
+  EXPECT_EQ(0.0, rot.Tan());
-	EXPECT_EQ(0.0, rot.Radians());
+  EXPECT_EQ(0.0, rot.Radians());
-	EXPECT_EQ(0.0, rot.Degrees());
+  EXPECT_EQ(0.0, rot.Degrees());
 
-	rot = frc5190::Rotation2d(1, 1, true);
+  rot = frc5190::Rotation2d(1, 1, true);
-	EXPECT_NEAR(std::sqrt(2) / 2, rot.Cos(), kTestEpsilon);
+  EXPECT_NEAR(std::sqrt(2) / 2, rot.Cos(), kTestEpsilon);
-	EXPECT_NEAR(std::sqrt(2) / 2, rot.Sin(), kTestEpsilon);
+  EXPECT_NEAR(std::sqrt(2) / 2, rot.Sin(), kTestEpsilon);
-	EXPECT_NEAR(1.0, rot.Tan(), kTestEpsilon);
+  EXPECT_NEAR(1.0, rot.Tan(), kTestEpsilon);
-	EXPECT_NEAR(45.0, rot.Degrees(), kTestEpsilon);
+  EXPECT_NEAR(45.0, rot.Degrees(), kTestEpsilon);
-	EXPECT_NEAR(frc5190::kPi / 4, rot.Radians(), kTestEpsilon);
+  EXPECT_NEAR(frc5190::kPi / 4, rot.Radians(), kTestEpsilon);
 
+  auto twist = frc5190::Twist2d();
 }

Tests/test.cpp

@@ -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);
-
-}