Fix compilation errors

This commit is contained in:
Prateek Machiraju 2019-06-24 16:52:41 -04:00
parent 5b291916d7
commit 3a0462cfb4
7 changed files with 172 additions and 85 deletions

107
.clang-format Normal file
View File

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

View File

@ -3,10 +3,11 @@
#include "VaryInterpolatable.h" #include "VaryInterpolatable.h"
#include "Translation2d.h" #include "Translation2d.h"
#include "Rotation2d.h" #include "Rotation2d.h"
#include "Twist2d.h"
#include "Utilities.h" #include "Utilities.h"
#include "Twist2d.h"
namespace frc5190 { namespace frc5190 {
class Twist2d;
class Pose2d final : public VaryInterpolatable<Pose2d> { class Pose2d final : public VaryInterpolatable<Pose2d> {
public: public:
// Constructors // Constructors
@ -20,7 +21,7 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
// Overriden Methods // Overriden Methods
double Distance(const Pose2d& other) override { 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 { Pose2d Interpolate(const Pose2d& end_value, const double t) override {
if (t <= 0) { if (t <= 0) {
@ -29,8 +30,8 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
if (t >= 1) { if (t >= 1) {
return end_value; 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 // Operator Overloads
@ -45,28 +46,6 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
const Translation2d& Translation() const { return translation_; } const Translation2d& Translation() const { return translation_; }
const Rotation2d& Rotation() const { return rotation_; } 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 { Pose2d Mirror() const {
return Pose2d{Translation2d{translation_.X(), 27.0 - translation_.Y()}, return Pose2d{Translation2d{translation_.X(), 27.0 - translation_.Y()},
-rotation_}; -rotation_};
@ -81,10 +60,52 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
if (!rotation_.IsParallel(other.rotation_)) { if (!rotation_.IsParallel(other.rotation_)) {
return false; 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); 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: private:
Translation2d translation_; Translation2d translation_;
Rotation2d rotation_; Rotation2d rotation_;

View File

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

View File

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

View File

@ -2,9 +2,6 @@
#include <cmath> #include <cmath>
#include "Pose2d.h"
#include "Utilities.h"
namespace frc5190 { namespace frc5190 {
class Twist2d { class Twist2d {
public: public:
@ -28,23 +25,6 @@ class Twist2d {
return std::hypot(dx_, dy_); 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: private:
double dx_; double dx_;
double dy_; double dy_;

View File

@ -1,22 +1,22 @@
#include "pch.h" #include "pch.h"
#include "../FalconLibraryCPP/src/Translation2d.h" #include "../FalconLibraryCPP/src/FalconLibrary.h"
#include "../FalconLibraryCPP/src/Rotation2d.h"
constexpr double kTestEpsilon = 1E-9; constexpr double kTestEpsilon = 1E-9;
TEST(TestRotation2d, TestRotation2d) { 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();
} }

View File

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