Start separating impls from declarations
This commit is contained in:
parent
2f1125bc59
commit
bf0e158d24
|
@ -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'
|
||||||
|
|
|
@ -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
|
|
|
@ -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
|
|
|
@ -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
|
|
|
@ -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
|
|
106
src/main/cpp/fl/mathematics/geometry/Pose2d.cpp
Normal file
106
src/main/cpp/fl/mathematics/geometry/Pose2d.cpp
Normal 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
|
38
src/main/cpp/fl/mathematics/geometry/Pose2dWithCurvature.cpp
Normal file
38
src/main/cpp/fl/mathematics/geometry/Pose2dWithCurvature.cpp
Normal 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
|
32
src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp
Normal file
32
src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp
Normal 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
|
20
src/main/cpp/fl/mathematics/geometry/Translation2d.cpp
Normal file
20
src/main/cpp/fl/mathematics/geometry/Translation2d.cpp
Normal 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
|
42
src/main/include/fl/mathematics/geometry/Pose2d.h
Normal file
42
src/main/include/fl/mathematics/geometry/Pose2d.h
Normal 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
|
|
@ -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
|
39
src/main/include/fl/mathematics/geometry/Rotation2d.h
Normal file
39
src/main/include/fl/mathematics/geometry/Rotation2d.h
Normal 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
|
58
src/main/include/fl/mathematics/geometry/Translation2d.h
Normal file
58
src/main/include/fl/mathematics/geometry/Translation2d.h
Normal 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
|
Loading…
Reference in New Issue
Block a user