From 3f0dfe6bba65a13c6b675801cde9eee14a45ac2b Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Sat, 29 Jun 2019 14:39:30 -0400 Subject: [PATCH] run clang format --- src/bench/bench_vector_creation.cpp | 4 +- .../mathematics/control/TrajectoryTracker.cpp | 14 +++---- .../fl/mathematics/geometry/Rotation2d.cpp | 40 ++++++++++++------- .../fl/mathematics/geometry/Translation2d.cpp | 21 ++++++---- .../cpp/fl/mathematics/geometry/Twist2d.cpp | 26 ++++++------ .../spline/ParametricQuinticHermiteSpline.cpp | 2 +- .../trajectory/TrajectoryGenerator.cpp | 19 ++++----- src/main/include/FalconLibrary.h | 2 +- .../mathematics/control/PurePursuitTracker.h | 7 ++-- .../include/fl/mathematics/geometry/Pose2d.h | 4 +- .../mathematics/trajectory/TimedTrajectory.h | 3 +- .../trajectory/TrajectoryGenerator.h | 10 ++--- src/main/include/fl/types/Interpolatable.h | 2 +- src/test/cpp/trajectory-tests.cpp | 27 ++++--------- 14 files changed, 89 insertions(+), 92 deletions(-) diff --git a/src/bench/bench_vector_creation.cpp b/src/bench/bench_vector_creation.cpp index 7f24668..2662dd1 100644 --- a/src/bench/bench_vector_creation.cpp +++ b/src/bench/bench_vector_creation.cpp @@ -24,8 +24,8 @@ struct ConstrainedPose { } ConstrainedPose(ConstrainedPose& other) = default; - ConstrainedPose& operator=(ConstrainedPose& other) { - //std::cout << "Copy operator called"; + ConstrainedPose& operator =(ConstrainedPose& other) { + // std::cout << "Copy operator called"; this->state = other.state; this->distance = other.distance; this->max_velocity = other.max_velocity; diff --git a/src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp b/src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp index feb46aa..5d2c6e8 100644 --- a/src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp +++ b/src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp @@ -13,9 +13,8 @@ TrajectoryTrackerOutput TrajectoryTracker::NextState(const Pose2d& curre if (iterator_ == nullptr) throw std::exception("Iterator was nullptr."); auto& iterator = *iterator_; - const auto dt = (units::unit_cast(previous_time_) < 0.0) - ? units::second_t{0.0} - : current_time - previous_time_; + const auto dt = + (units::unit_cast(previous_time_) < 0.0) ? units::second_t{0.0} : current_time - previous_time_; previous_time_ = current_time; iterator.Advance(dt); @@ -33,9 +32,8 @@ TrajectoryTrackerOutput TrajectoryTracker::NextState(const Pose2d& curre const auto _dt = units::unit_cast(dt); const TrajectoryTrackerOutput output{ - linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity, - (angular_velocity - previous_velocity_->angular_velocity) / _dt - }; + linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity, + (angular_velocity - previous_velocity_->angular_velocity) / _dt}; previous_velocity_->linear_velocity = linear_velocity; previous_velocity_->angular_velocity = angular_velocity; @@ -47,5 +45,7 @@ TrajectorySamplePoint> TrajectoryTracker::Refere return iterator_->CurrentState(); } -bool TrajectoryTracker::IsFinished() const { return iterator_->IsDone(); } +bool TrajectoryTracker::IsFinished() const { + return iterator_->IsDone(); } +} // namespace fl diff --git a/src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp b/src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp index dc219e0..5b0913f 100644 --- a/src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp +++ b/src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp @@ -25,26 +25,38 @@ Rotation2d Rotation2d::FromDegrees(const double degrees) { return Rotation2d(Deg2Rad(degrees)); } -Rotation2d Rotation2d::operator-(const Rotation2d& other) const { return *this + -other; } - -Rotation2d Rotation2d::operator-() const { return Rotation2d(-value_); } - -Rotation2d Rotation2d::operator+(const Rotation2d& other) const { - return Rotation2d{ - Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(), - true - }; +Rotation2d Rotation2d::operator-(const Rotation2d& other) const { + return *this + -other; } -double Rotation2d::Radians() const { return value_; } +Rotation2d Rotation2d::operator-() const { + return Rotation2d(-value_); +} -double Rotation2d::Degrees() const { return Rad2Deg(value_); } +Rotation2d Rotation2d::operator+(const Rotation2d& other) const { + return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(), + true}; +} -double Rotation2d::Cos() const { return cos_; } +double Rotation2d::Radians() const { + return value_; +} -double Rotation2d::Sin() const { return sin_; } +double Rotation2d::Degrees() const { + return Rad2Deg(value_); +} -double Rotation2d::Tan() const { return sin_ / cos_; } +double Rotation2d::Cos() const { + return cos_; +} + +double Rotation2d::Sin() const { + return sin_; +} + +double Rotation2d::Tan() const { + return sin_ / cos_; +} bool Rotation2d::IsParallel(const Rotation2d& other) const { return EpsilonEquals((*this - other).Radians(), 0.0); diff --git a/src/main/cpp/fl/mathematics/geometry/Translation2d.cpp b/src/main/cpp/fl/mathematics/geometry/Translation2d.cpp index 170d15a..2d126cf 100644 --- a/src/main/cpp/fl/mathematics/geometry/Translation2d.cpp +++ b/src/main/cpp/fl/mathematics/geometry/Translation2d.cpp @@ -34,22 +34,27 @@ Translation2d Translation2d::operator*(const double scalar) const { } Translation2d Translation2d::operator*(const Rotation2d& rotation) const { - return Translation2d{ - x_ * rotation.Cos() - y_ * rotation.Sin(), - x_ * rotation.Sin() + y_ * rotation.Cos() - }; + return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(), x_ * rotation.Sin() + y_ * rotation.Cos()}; } Translation2d Translation2d::operator/(const double scalar) const { return Translation2d{X() / scalar, Y() / scalar}; } -Translation2d Translation2d::operator-() const { return Translation2d{-X(), -Y()}; } +Translation2d Translation2d::operator-() const { + return Translation2d{-X(), -Y()}; +} -double Translation2d::X() const { return x_; } +double Translation2d::X() const { + return x_; +} -double Translation2d::Y() const { return y_; } +double Translation2d::Y() const { + return y_; +} -double Translation2d::Norm() const { return std::hypot(x_, y_); } +double Translation2d::Norm() const { + return std::hypot(x_, y_); +} } // namespace fl diff --git a/src/main/cpp/fl/mathematics/geometry/Twist2d.cpp b/src/main/cpp/fl/mathematics/geometry/Twist2d.cpp index f26368c..19a260a 100644 --- a/src/main/cpp/fl/mathematics/geometry/Twist2d.cpp +++ b/src/main/cpp/fl/mathematics/geometry/Twist2d.cpp @@ -2,30 +2,28 @@ namespace fl { -Twist2d::Twist2d() - : dx_(0.0), - dy_(0.0), - dtheta_(0.0) { -} +Twist2d::Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {} -Twist2d::Twist2d(const double dx, const double dy, const double dtheta) - : dx_(dx), - dy_(dy), - dtheta_(dtheta) { -} +Twist2d::Twist2d(const double dx, const double dy, const double dtheta) : dx_(dx), dy_(dy), dtheta_(dtheta) {} Twist2d Twist2d::operator*(const double scalar) const { return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar}; } -double Twist2d::Dx() const { return dx_; } +double Twist2d::Dx() const { + return dx_; +} -double Twist2d::Dy() const { return dy_; } +double Twist2d::Dy() const { + return dy_; +} -double Twist2d::Dtheta() const { return dtheta_; } +double Twist2d::Dtheta() const { + return dtheta_; +} double Twist2d::Norm() const { if (dy_ == 0.0) return std::abs(dx_); return std::hypot(dx_, dy_); } -} +} // namespace fl diff --git a/src/main/cpp/fl/mathematics/spline/ParametricQuinticHermiteSpline.cpp b/src/main/cpp/fl/mathematics/spline/ParametricQuinticHermiteSpline.cpp index bc75a46..2e0663e 100644 --- a/src/main/cpp/fl/mathematics/spline/ParametricQuinticHermiteSpline.cpp +++ b/src/main/cpp/fl/mathematics/spline/ParametricQuinticHermiteSpline.cpp @@ -62,7 +62,7 @@ double ParametricQuinticHermiteSpline::Curvature(const double t) const { double ParametricQuinticHermiteSpline::DCurvature(const double t) const { const auto dx_2dy2 = Dx(t) * Dx(t) + Dy(t) * Dy(t); const auto num = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 - - 3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * (Dx(t) * Ddx(t) + Dy(t) * Ddy(t)); + 3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * (Dx(t) * Ddx(t) + Dy(t) * Ddy(t)); return num / (dx_2dy2 * dx_2dy2 * std::sqrt(dx_2dy2)); } diff --git a/src/main/cpp/fl/mathematics/trajectory/TrajectoryGenerator.cpp b/src/main/cpp/fl/mathematics/trajectory/TrajectoryGenerator.cpp index 67a9f88..d6f9df9 100644 --- a/src/main/cpp/fl/mathematics/trajectory/TrajectoryGenerator.cpp +++ b/src/main/cpp/fl/mathematics/trajectory/TrajectoryGenerator.cpp @@ -1,15 +1,10 @@ #include "fl/mathematics/trajectory/TrajectoryGenerator.h" namespace fl { -TimedTrajectory TrajectoryGenerator::GenerateTrajectory(std::vector waypoints, - const Constraints& constraints, - const double - start_velocity, - const double end_velocity, - const double max_velocity, - const double - max_acceleration, - const bool reversed) { +TimedTrajectory TrajectoryGenerator::GenerateTrajectory( + std::vector waypoints, const Constraints& constraints, const double start_velocity, + const double end_velocity, const double max_velocity, const double max_acceleration, + const bool reversed) { const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)}; if (reversed) { @@ -35,14 +30,14 @@ TimedTrajectory TrajectoryGenerator::GenerateTrajectory(std } IndexedTrajectory TrajectoryGenerator::TrajectoryFromSplineWaypoints( - const std::vector& waypoints, const double max_dx, const double max_dy, const double max_dtheta) { + const std::vector& waypoints, const double max_dx, const double max_dy, const double max_dtheta) { std::vector> splines(waypoints.size() - 1); for (auto i = 1; i < waypoints.size(); ++i) { splines[i - 1] = std::make_shared(waypoints[i - 1], waypoints[i]); } auto trajectory = IndexedTrajectory( - SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta)); + SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta)); return trajectory; } -} +} // namespace fl diff --git a/src/main/include/FalconLibrary.h b/src/main/include/FalconLibrary.h index b4a07a6..d40ca9c 100644 --- a/src/main/include/FalconLibrary.h +++ b/src/main/include/FalconLibrary.h @@ -8,9 +8,9 @@ #include "fl/mathematics/geometry/Translation2d.h" #include "fl/mathematics/geometry/Twist2d.h" +#include "fl/mathematics/control/PurePursuitTracker.h" #include "fl/mathematics/control/RamseteTracker.h" #include "fl/mathematics/control/TrajectoryTracker.h" -#include "fl/mathematics/control/PurePursuitTracker.h" #include "fl/mathematics/spline/ParametricQuinticHermiteSpline.h" #include "fl/mathematics/spline/ParametricSpline.h" diff --git a/src/main/include/fl/mathematics/control/PurePursuitTracker.h b/src/main/include/fl/mathematics/control/PurePursuitTracker.h index 190ef2a..c89990b 100644 --- a/src/main/include/fl/mathematics/control/PurePursuitTracker.h +++ b/src/main/include/fl/mathematics/control/PurePursuitTracker.h @@ -11,13 +11,12 @@ class PurePursuitTracker : public TrajectoryTracker { const double min_lookahead_distance); TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator& iterator, - const Pose2d& robot_pose) const - override; + const Pose2d& robot_pose) const override; private: - double lat_; + double lat_; units::second_t lookahead_time_; - double min_lookahead_distance_; + double min_lookahead_distance_; Pose2d CalculateLookaheadPose(const TimedIterator& iterator, const Pose2d& robot_pose) const; diff --git a/src/main/include/fl/mathematics/geometry/Pose2d.h b/src/main/include/fl/mathematics/geometry/Pose2d.h index c437c90..7bff1ee 100644 --- a/src/main/include/fl/mathematics/geometry/Pose2d.h +++ b/src/main/include/fl/mathematics/geometry/Pose2d.h @@ -28,12 +28,12 @@ class Pose2d final : public VaryInterpolatable { // Member Functions Pose2d Mirror() const; Pose2d TransformBy(const Pose2d& other) const; - bool IsCollinear(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); + static Pose2d FromTwist(const Twist2d& twist); private: Translation2d translation_; diff --git a/src/main/include/fl/mathematics/trajectory/TimedTrajectory.h b/src/main/include/fl/mathematics/trajectory/TimedTrajectory.h index 9720366..ff499af 100644 --- a/src/main/include/fl/mathematics/trajectory/TimedTrajectory.h +++ b/src/main/include/fl/mathematics/trajectory/TimedTrajectory.h @@ -28,8 +28,7 @@ class TimedEntry final : public VaryInterpolatable> { if (delta_t < 0_s) return end_value.Interpolate(*this, 1.0 - t); - auto reversing = velocity_ < 0_mps || - EpsilonEquals(velocity_, 0_mps) && acceleration_ < 0_mps_sq; + auto reversing = velocity_ < 0_mps || EpsilonEquals(velocity_, 0_mps) && acceleration_ < 0_mps_sq; units::meters_per_second_t new_v = velocity_ + acceleration_ * delta_t; units::meter_t new_s = diff --git a/src/main/include/fl/mathematics/trajectory/TrajectoryGenerator.h b/src/main/include/fl/mathematics/trajectory/TrajectoryGenerator.h index 0a25396..dc5136b 100644 --- a/src/main/include/fl/mathematics/trajectory/TrajectoryGenerator.h +++ b/src/main/include/fl/mathematics/trajectory/TrajectoryGenerator.h @@ -17,13 +17,13 @@ class TrajectoryGenerator { public: static TimedTrajectory GenerateTrajectory( - std::vector waypoints, const Constraints& constraints, const double start_velocity, - const double end_velocity, const double max_velocity, const double max_acceleration, - const bool reversed); + std::vector waypoints, const Constraints& constraints, const double start_velocity, + const double end_velocity, const double max_velocity, const double max_acceleration, + const bool reversed); static IndexedTrajectory TrajectoryFromSplineWaypoints( - const std::vector& waypoints, const double max_dx, const double max_dy, - const double max_dtheta); + const std::vector& waypoints, const double max_dx, const double max_dy, + const double max_dtheta); template struct ConstrainedPose { diff --git a/src/main/include/fl/types/Interpolatable.h b/src/main/include/fl/types/Interpolatable.h index 8f2dc20..8abf1d1 100644 --- a/src/main/include/fl/types/Interpolatable.h +++ b/src/main/include/fl/types/Interpolatable.h @@ -9,7 +9,7 @@ class Interpolatable { virtual ~Interpolatable() = default; virtual T Interpolate(const T& end_value, double t) const = 0; - template + template static constexpr T Lerp(const T& start_value, const T& end_value, const double t) { return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0); } diff --git a/src/test/cpp/trajectory-tests.cpp b/src/test/cpp/trajectory-tests.cpp index cd1e5e2..36c97d1 100644 --- a/src/test/cpp/trajectory-tests.cpp +++ b/src/test/cpp/trajectory-tests.cpp @@ -7,30 +7,20 @@ class TrajectoryTest : public ::testing::Test { public: TrajectoryTest() {} - void Run(fl::Pose2d initial, - fl::Pose2d final, - double max_velocity = 3, - double max_acceleration = 2, + void Run(fl::Pose2d initial, fl::Pose2d final, double max_velocity = 3, double max_acceleration = 2, bool backwards = false) { auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory( std::vector{initial, final}, - std::vector*>{}, - 0.0, - 0.0, - max_velocity, - max_acceleration, - backwards); + std::vector*>{}, 0.0, 0.0, max_velocity, + max_acceleration, backwards); auto pose = trajectory.Sample(units::second_t(0.0)).state.State().Pose(); EXPECT_FALSE(false); - EXPECT_NEAR( - pose.Translation().X(), initial.Translation().X(), kTestEpsilon); - EXPECT_NEAR( - pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon); - EXPECT_NEAR( - pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon); + EXPECT_NEAR(pose.Translation().X(), initial.Translation().X(), kTestEpsilon); + EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon); + EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon); const auto iterator = trajectory.Iterator(); @@ -38,7 +28,7 @@ class TrajectoryTest : public ::testing::Test { while (!iterator->IsDone()) { auto prev_sample = sample; - sample = iterator->Advance(0.02_s); + sample = iterator->Advance(0.02_s); EXPECT_LT(std::abs(units::unit_cast(sample.state.Velocity())), max_velocity + kTestEpsilon); EXPECT_LT(std::abs(units::unit_cast(sample.state.Acceleration())), @@ -55,8 +45,7 @@ class TrajectoryTest : public ::testing::Test { EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon); EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon); - EXPECT_NEAR( - pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon); + EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon); } };