Fix compilation errors
This commit is contained in:
parent
5b291916d7
commit
3a0462cfb4
107
.clang-format
Normal file
107
.clang-format
Normal 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
|
||||
...
|
|
@ -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();
|
||||
return *this + (twist * t).AsPose();
|
||||
const auto twist = ToTwist(-*this + end_value);
|
||||
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_;
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -51,8 +51,7 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
|
|||
x_ * rotation.Sin() + y_ * rotation.Cos()};
|
||||
}
|
||||
|
||||
Translation2d
|
||||
operator/(const double scalar) const {
|
||||
Translation2d operator/(const double scalar) const {
|
||||
return Translation2d{X() / scalar, Y() / scalar};
|
||||
}
|
||||
|
||||
|
|
|
@ -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_;
|
||||
|
|
|
@ -1,6 +1,5 @@
|
|||
#include "pch.h"
|
||||
#include "../FalconLibraryCPP/src/Translation2d.h"
|
||||
#include "../FalconLibraryCPP/src/Rotation2d.h"
|
||||
#include "../FalconLibraryCPP/src/FalconLibrary.h"
|
||||
|
||||
constexpr double kTestEpsilon = 1E-9;
|
||||
|
||||
|
@ -19,4 +18,5 @@ TEST(TestRotation2d, TestRotation2d) {
|
|||
EXPECT_NEAR(45.0, rot.Degrees(), kTestEpsilon);
|
||||
EXPECT_NEAR(frc5190::kPi / 4, rot.Radians(), kTestEpsilon);
|
||||
|
||||
auto twist = frc5190::Twist2d();
|
||||
}
|
|
@ -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);
|
||||
|
||||
}
|
Loading…
Reference in New Issue
Block a user