Start separating impls from declarations

This commit is contained in:
Prateek Machiraju 2019-06-29 12:39:42 -04:00
parent 2f1125bc59
commit bf0e158d24
34 changed files with 370 additions and 266 deletions

View File

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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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