Start separating impls from declarations

build.gradle

@@ -9,11 +9,11 @@ model {
         falconlibrary(NativeLibrarySpec) {
             sources.cpp {
                 source {
-                    srcDir "src/cpp"
+                    srcDir "src/main/cpp"
                     include "**/*.cpp"
                 }
                 exportedHeaders {
-                    srcDir "src/include"
+                    srcDir "src/main/include"
                     include "**/*.h"
                 }
                 lib project: ':libs', library: 'units', linkage: 'static'

src/include/fl/mathematics/geometry/Pose2d.h

@@ -1,106 +0,0 @@
-#pragma once
-
-#include "Rotation2d.h"
-#include "Translation2d.h"
-#include "Twist2d.h"
-#include "fl/Utilities.h"
-
-namespace fl {
-class Twist2d;
-class Pose2d final : public VaryInterpolatable<Pose2d> {
- 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) const override { return ToTwist(-*this + other).Norm(); }
-  Pose2d Interpolate(const Pose2d& end_value, const double t) const override {
-    if (t <= 0) {
-      return *this;
-    }
-    if (t >= 1) {
-      return end_value;
-    }
-    const auto twist = ToTwist(-*this + end_value);
-    return *this + FromTwist(twist * t);
-  }
-
-  // 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_; }
-
-  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 = ToTwist(-(*this) + other);
-    return EpsilonEquals(twist.Dy(), 0.0) && EpsilonEquals(twist.Dtheta(), 0.0);
-  }
-
-  Pose2d InFrameOfReferenceOf(const Pose2d& other) const { return (-other) + *this; }
-
-  // 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_;
-};
-}  // namespace fl

src/include/fl/mathematics/geometry/Pose2dWithCurvature.h

@@ -1,40 +0,0 @@
-#pragma once
-
-#include <utility>
-#include "Pose2d.h"
-
-namespace fl {
-class Pose2dWithCurvature final : public VaryInterpolatable<Pose2dWithCurvature> {
- public:
-  // Constructors
-  Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds)
-      : pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {}
-
-  Pose2dWithCurvature() : pose_(Pose2d{}), curvature_(0.0), dkds_(0.0) {}
-
-  // Overriden Methods
-  double Distance(const Pose2dWithCurvature& other) const override { return pose_.Distance(other.pose_); }
-
-  Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value, double t) const 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 fl

src/include/fl/mathematics/geometry/Rotation2d.h

@@ -1,55 +0,0 @@
-#pragma once
-
-#include <cmath>
-#include "fl/Utilities.h"
-
-namespace fl {
-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 fl

src/include/fl/mathematics/geometry/Translation2d.h

@@ -1,63 +0,0 @@
-#pragma once
-
-#include <cmath>
-
-#include "Rotation2d.h"
-#include "fl/types/VaryInterpolatable.h"
-
-namespace fl {
-
-class Translation2d final : public VaryInterpolatable<Translation2d> {
- 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) const override {
-    return std::hypot(other.X() - X(), other.Y() - Y());
-  }
-
-  Translation2d Interpolate(const Translation2d& end_value, const double t) const 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_; }
-
-  double Norm() const { return std::hypot(x_, y_); }
-
- private:
-  double x_;
-  double y_;
-};
-}  // namespace fl

src/main/cpp/fl/mathematics/geometry/Pose2d.cpp

@@ -0,0 +1,106 @@
+#include "fl/mathematics/geometry/Pose2d.h"
+
+namespace fl {
+
+Pose2d::Pose2d() : translation_(Translation2d()), rotation_(Rotation2d()) {}
+
+Pose2d::Pose2d(Translation2d translation, const Rotation2d rotation)
+    : translation_(std::move(translation)), rotation_(rotation) {}
+
+Pose2d::Pose2d(const double x, const double y, const Rotation2d rotation)
+    : translation_(Translation2d(x, y)), rotation_(rotation) {}
+
+double Pose2d::Distance(const Pose2d& other) const {
+  return ToTwist(-*this + other).Norm();
+}
+
+Pose2d Pose2d::Interpolate(const Pose2d& end_value, const double t) const {
+  if (t <= 0) {
+    return *this;
+  }
+  if (t >= 1) {
+    return end_value;
+  }
+  const auto twist = ToTwist(-*this + end_value);
+  return *this + FromTwist(twist * t);
+}
+
+Pose2d Pose2d::operator+(const Pose2d& other) const {
+  return TransformBy(other);
+}
+
+Pose2d Pose2d::operator-(const Pose2d& other) const {
+  return TransformBy(-other);
+}
+
+Pose2d Pose2d::operator-() const {
+  const auto inverted_rotation = -rotation_;
+  return Pose2d{(-translation_) * inverted_rotation, inverted_rotation};
+}
+
+const Translation2d& Pose2d::Translation() const {
+  return translation_;
+}
+
+const Rotation2d& Pose2d::Rotation() const {
+  return rotation_;
+}
+
+Pose2d Pose2d::Mirror() const {
+  return Pose2d{Translation2d{translation_.X(), 27.0 - translation_.Y()}, -rotation_};
+}
+
+Pose2d Pose2d::TransformBy(const Pose2d& other) const {
+  return Pose2d{translation_ + (other.translation_ * rotation_), rotation_ + other.rotation_};
+}
+
+bool Pose2d::IsCollinear(const Pose2d& other) const {
+  if (!rotation_.IsParallel(other.rotation_)) {
+    return false;
+  }
+  const auto twist = ToTwist(-(*this) + other);
+  return EpsilonEquals(twist.Dy(), 0.0) && EpsilonEquals(twist.Dtheta(), 0.0);
+}
+
+Pose2d Pose2d::InFrameOfReferenceOf(const Pose2d& other) const {
+  return (-other) + *this;
+}
+
+Twist2d Pose2d::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()};
+}
+
+Pose2d 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}};
+}
+
+}  // namespace fl

src/main/cpp/fl/mathematics/geometry/Pose2dWithCurvature.cpp

@@ -0,0 +1,38 @@
+#include "fl/mathematics/geometry/Pose2dWithCurvature.h"
+
+namespace fl {
+Pose2dWithCurvature::Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds)
+    : pose_(std::move(pose)), curvature_(curvature), dkds_(dkds) {}
+
+Pose2dWithCurvature::Pose2dWithCurvature() : pose_(Pose2d{}), curvature_(0.0), dkds_(0.0) {}
+
+double Pose2dWithCurvature::Distance(const Pose2dWithCurvature& other) const {
+  return pose_.Distance(other.pose_);
+}
+
+Pose2dWithCurvature Pose2dWithCurvature::Interpolate(const Pose2dWithCurvature& end_value, double t) const {
+  return Pose2dWithCurvature{pose_.Interpolate(end_value.pose_, t), Lerp(curvature_, end_value.curvature_, t),
+                             Lerp(dkds_, end_value.dkds_, t)};
+}
+
+Pose2dWithCurvature Pose2dWithCurvature::operator+(const Pose2d& other) const {
+  return Pose2dWithCurvature{pose_ + other, curvature_, dkds_};
+}
+
+const Pose2d& Pose2dWithCurvature::Pose() const {
+  return pose_;
+}
+
+double Pose2dWithCurvature::Curvature() const {
+  return curvature_;
+}
+
+double Pose2dWithCurvature::Dkds() const {
+  return dkds_;
+}
+
+Pose2dWithCurvature Pose2dWithCurvature::Mirror() const {
+  return Pose2dWithCurvature{pose_.Mirror(), -curvature_, -dkds_};
+}
+
+}  // namespace fl

src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp

@@ -0,0 +1,32 @@
+#include "fl/mathematics/geometry/Rotation2d.h"
+
+namespace fl {
+
+Rotation2d::Rotation2d() : value_(0.0), cos_(1.0), sin_(0.0) {}
+Rotation2d::Rotation2d(const double value) : value_(value), cos_(std::cos(value)), sin_(std::sin(value)) {}
+Rotation2d::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_);
+}
+
+Rotation2d Rotation2d::FromDegrees(const double degrees) {
+  return Rotation2d(Deg2Rad(degrees));
+}
+
+bool Rotation2d::IsParallel(const Rotation2d& other) const {
+  return EpsilonEquals((*this - other).Radians(), 0.0);
+}
+
+}  // namespace fl

src/main/cpp/fl/mathematics/geometry/Translation2d.cpp

@@ -0,0 +1,20 @@
+#include "fl/mathematics/geometry/Translation2d.h"
+
+namespace fl {
+
+Translation2d::Translation2d() : x_(0.0), y_(0.0) {}
+Translation2d::Translation2d(const double x, const double y) : x_(x), y_(y) {}
+Translation2d::Translation2d(const double length, const Rotation2d& rotation)
+    : x_(length * rotation.Cos()), y_(length * rotation.Sin()) {}
+
+Translation2d Translation2d::Interpolate(const Translation2d& end_value, const double t) const {
+  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)};
+}
+
+}  // namespace fl

src/main/include/fl/mathematics/geometry/Pose2d.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include "Rotation2d.h"
+#include "Translation2d.h"
+#include "Twist2d.h"
+
+namespace fl {
+class Pose2d final : public VaryInterpolatable<Pose2d> {
+ public:
+  // Constructors
+  Pose2d();
+  Pose2d(Translation2d translation, const Rotation2d rotation);
+  Pose2d(const double x, const double y, const Rotation2d rotation);
+
+  // Overriden Methods
+  double Distance(const Pose2d& other) const override;
+  Pose2d Interpolate(const Pose2d& end_value, const double t) const override;
+
+  // Operator Overloads
+  Pose2d operator+(const Pose2d& other) const;
+  Pose2d operator-(const Pose2d& other) const;
+  Pose2d operator-() const;
+
+  // Accessors
+  const Translation2d& Translation() const;
+  const Rotation2d&    Rotation() const;
+
+  // Member Functions
+  Pose2d Mirror() const;
+  Pose2d TransformBy(const Pose2d& other) const;
+  bool IsCollinear(const Pose2d& other) const;
+  Pose2d InFrameOfReferenceOf(const Pose2d& other) const;
+
+  // Static Methods
+  static Twist2d ToTwist(const Pose2d& pose);
+  static Pose2d FromTwist(const Twist2d& twist);
+
+ private:
+  Translation2d translation_;
+  Rotation2d    rotation_;
+};
+}  // namespace fl

src/main/include/fl/mathematics/geometry/Pose2dWithCurvature.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <utility>
+#include "Pose2d.h"
+
+namespace fl {
+class Pose2dWithCurvature final : public VaryInterpolatable<Pose2dWithCurvature> {
+ public:
+  // Constructors
+  Pose2dWithCurvature();
+  Pose2dWithCurvature(Pose2d pose, const double curvature, const double dkds);
+
+  // Overriden Methods
+  double Distance(const Pose2dWithCurvature& other) const override;
+
+  Pose2dWithCurvature Interpolate(const Pose2dWithCurvature& end_value, double t) const override;
+
+  // Operator Overloads
+  Pose2dWithCurvature operator+(const Pose2d& other) const;
+
+  // Accessors
+  const Pose2d& Pose() const;
+  double        Curvature() const;
+  double        Dkds() const;
+
+  Pose2dWithCurvature Mirror() const;
+
+ private:
+  Pose2d pose_;
+  double curvature_;
+  double dkds_;
+};
+}  // namespace fl

src/main/include/fl/mathematics/geometry/Rotation2d.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <cmath>
+#include "fl/Utilities.h"
+
+namespace fl {
+class Rotation2d final {
+ public:
+  // Constructors
+  Rotation2d();
+  Rotation2d(const double x, const double y, const bool normalize);
+  explicit Rotation2d(const double value);
+  
+  static Rotation2d FromDegrees(const double degrees);
+
+  // Operator Overloads
+  inline Rotation2d operator-(const Rotation2d& other) const { return *this + -other; }
+  inline Rotation2d operator-() const { return Rotation2d(-value_); }
+
+  inline Rotation2d operator+(const Rotation2d& other) const {
+    return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(),
+                      true};
+  }
+
+  // Accessors
+  inline double Radians() const { return value_; }
+  inline double Degrees() const { return Rad2Deg(value_); }
+  inline double Cos() const { return cos_; }
+  inline double Sin() const { return sin_; }
+  inline double Tan() const { return sin_ / cos_; }
+
+  bool IsParallel(const Rotation2d& other) const;
+
+ private:
+  double value_;
+  double cos_;
+  double sin_;
+};
+}  // namespace fl

src/main/include/fl/mathematics/geometry/Translation2d.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <cmath>
+
+#include "Rotation2d.h"
+#include "fl/types/VaryInterpolatable.h"
+
+namespace fl {
+
+class Translation2d final : public VaryInterpolatable<Translation2d> {
+ public:
+  // Constructors
+  Translation2d();
+  Translation2d(const double x, const double y);
+  Translation2d(const double length, const Rotation2d& rotation);
+
+  // Overriden Methods
+  inline double Distance(const Translation2d& other) const override {
+    return std::hypot(other.X() - X(), other.Y() - Y());
+  }
+
+  Translation2d Interpolate(const Translation2d& end_value, const double t) const override;
+
+  // Operator Overloads
+  inline Translation2d operator+(const Translation2d& other) const {
+    return Translation2d{X() + other.X(), Y() + other.Y()};
+  }
+
+  inline Translation2d operator-(const Translation2d& other) const {
+    return Translation2d{X() - other.X(), Y() - other.Y()};
+  }
+
+  inline Translation2d operator*(const double scalar) const {
+    return Translation2d{X() * scalar, Y() * scalar};
+  }
+
+  inline Translation2d operator*(const Rotation2d& rotation) const {
+    return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(),
+                         x_ * rotation.Sin() + y_ * rotation.Cos()};
+  }
+
+  inline Translation2d operator/(const double scalar) const {
+    return Translation2d{X() / scalar, Y() / scalar};
+  }
+
+  inline Translation2d operator-() const { return Translation2d{-X(), -Y()}; }
+
+  // Accessors
+  inline double X() const { return x_; }
+  inline double Y() const { return y_; }
+
+  inline double Norm() const { return std::hypot(x_, y_); }
+
+ private:
+  double x_;
+  double y_;
+};
+}  // namespace fl